Avoiding Obstacles

A small recap of my Master Thesis. I’m trying to improve the localization of a robot using DASH7 tags. The idea is that I can localize a robot using a laser and a map of the environment but that localization is somewhat inaccurate. Using the DASH7 tags I can tell the robot more about its environment and thus increase the localization.

But before all that can happen the robot first needs to drive around autonomously without hitting any static (closet, chair, table,…etc) or dynamic objects (humans, cats, dogs,…etc). This is the first part of my research, we already know that we are going to use the VFH algorithm now an important step is to configure it.

Heading over to the Player/Stage manually I found out that the VFH algorithm is implemented and allows to configure almost every calculation resulting in a staggering 25+ different parameters, holy cow.

Settings up the different tests

I decided to test two different parts, first the robot should drive to a specific location. When using the VFH algorithm we get a new interface (planner) that allows to send the robot to a specific location relative to its starting position (which is always 0,0).

The first test should drive the robot to a specific location (1,1). The second test will see if the robot can drive sequentially to different coordinates and the robot drives in a square.

The next three tests deal with obstacle avoidance. All the test are quite easy, the robot drives 5m straight and I place different objects in front of it and the robot should avoid them. If the robot drives into the objects or fails to avoid them in a reasonable time the parameters are changed and tested again.

Firstly, we should test and see if the robot can avoid typical domestic objects. I tested a flowerpot, closet, table with chairs, chairs and anything else I could find. Secondly, we should test and see if the robot can pass through a door (typically 80cm here in Belgium). Finally, we also test dynamic objects, I walk towards the robot and the robot should avoid me.

Results and Parameters changed

I’m only going to talk about my findings and what parameters I changed to avoid a typical obstacle. If I don’t mention a parameter it means I never changed it and left the value default.

max_speed

I first changed this to 1.2 since the Pioneer robot could handle around 1.2m/s, boy I couldn’t be more wrong. I got a lot of warnings about exceeding speed and velocity and other crap in the console. After lowering the value drastically I ended up with a save 0.4 which is two times more than the default. In reality the robot drives blazing fast around obstacles so I’m happy.

safety_dist

The safety distance determines what distance the robot must keep from an object at all times (regardless of the free_space_cutoff). It comes in two versions, when standing still and moving at 1m/s. This parameter will greatly influence if the robot pass between two objects or not. If I change both to 0.10 (default) then the robot can pass between a door opening, and a chair. If I increase it to 0.20 it won’t go under the chair and avoids it but cannot pass between a door.

free_space_cutoff

The parameters come in two version, when standing still and when moving at 1m/s. The parameters influences when the robot starts avoiding an object. A high value (2000000.0) will drive the robot nearly against the obstacle before avoiding it while a lower value will make sure that the robot starts early with the obstacle avoiding. The parameter doesn’t have a unit so it’s a bit guessing, I picked the following values based on my test results: free_space_cutoff_1ms 100000 & free_space_cutoff_0ms 200000.

min_turnrate & max_turnrate

I changed them and then I got a lot of warnings about right wheel velocity and left wheel velocity. Not that good so I left them default.

distance_epsilon & angle_epsilon

When the robot drives a goal the distance and angle epsilon will determine what error is acceptable. If I increase the parameters I don’t really care if the robot arrives exactly at the goal, if I lower both values then I want a precise result (based on the slipping odometry). The robot will keep driving around and turn till it arrives at it goal. So with a higher precision it can take a while till the robot thinks its at the goal. I increased the accuracy: distance_epsilon 0.2 & angle_epsilon 5.

Remarks about test results

So all the test succeeded and the robot could avoid all the obstacles. A big problem remains, as mentioned in the safety_dist parameter, is that the robot can avoid a table with chairs (using 0.2 for safety_dist) but cannot through a door. Change the parameter so the robot can go through a door (using 0.1 for safety_dist) then the robot will also attempt to go through the table with chairs and gets stuck.

I still have to test and see if any other parameters can solve this problem. I’m thinking about obs_cutoff that doesn’t have an explanation on the VFH page of Player project.

The last test was a person walk towards the robot also works quite well. The VFH algorithm doesn’t take into account the velocity of objects, so a moving person towards the robot is quite difficult. The test showed that the person also needs to take a small step in a direction (right or left) and the robot will do the same. If you walk straight then you step on the robot or the robot drives over your toes.

Conclusion

The robot can avoid static and dynamic objects. If I found a solution for my door-table problem then I’ll post an update (it is quite important for a domestic environment).

The post Vector Field Histogram (VFH) Player/Stage appeared first on Fritz-Hut.

• A starting position and orientation has to be given (not really a requirement but the algorithm tends to fail else)
• Particles can move through walls or be placed on walls
• The initial particle distribution does not fill the complete map

This makes the algorithm quite useless. I have two options concerning my Master Thesis, create my own or change it so it does work as expected. I decided to create my particle filter from scratch, later on I’ll decide if I’ll adapt the AMCL driver or create my own. The code is based on the online lessons from Udacity (by Sebastian Thrun).

The setup of the particle filter

The particle filter setup, including 6 landmarks

So we have a small area, the robot and 6 landmarks. As you can see is the Field of View (FOV) big enough so I always see one landmark.

Bits and pieces

So we got everything set up now the code. I started from the original python code from Udacity that I created. Now I just had to port the code from python to C++ Player/Stage style.

Obstacle Avoidance & Random Driving

First I created some code so the robot could drive around (random) and avoid the walls in the setup. I decided to keep it very simple and basic. The wander code looks as follows:

void Wander (double *forwardSpeed ,double *turnSpeed)
{
  int maxSpeed = 1;
  int maxTurn = 60;
  double fspeed, tspeed;
  // fspeed is between 0 and 10
  fspeed = rand()%11;
  // (fspeed/10) is between 0 and 1
  fspeed = (fspeed/10)*maxSpeed;
  tspeed = rand()%(2*maxTurn);
  tspeed = tspeed-maxTurn;
  // tspeed is between -maxTurn and +maxTurn
  *forwardSpeed = fspeed;
  *turnSpeed = tspeed;
}

So we create random variables and see that they are between the right ranges. Speed in player stage must always be between 0-1 and the turn speed should be in radians. The following code I used to avoiding obstacles (walls):

void AvoidObstacles(double *forwardSpeed, double *turnSpeed, RangerProxy &rp) {

  double avoidDistance = 0.8;
  int avoidTurnSpeed = 50;
  double currDistance = 0.0;

  if(rp.GetRangeCount() == 0) {
    error("GetRangeCount of Sonars == 0");
    return;
  }

  for(uint32_t i = 1; i <= rp.GetRangeCount(); i++) {
    currDistance = rp.GetRange(i-1);
    if(currDistance < avoidDistance) {
      if(i < 4) {
	*turnSpeed = -avoidTurnSpeed;
      } else {
	*turnSpeed = avoidTurnSpeed;
      }
      return;
    }
  }

}

Again very basic, it works but that’s about it. Note the check I made to see if we got any range sensors (rp.GetRangeCount()). For some reason it’s possible that player crashes during the first iteration of your while/for loop because the ranger sensors are not completely initialized.

Helpful bits and pieces

I decided to keep track of my particles in a struct (pf_particle_t), it allows to easily add additional variables if need, my particles contain the following data: (x, y, yaw, weight). Next I should initialize an amount of particles with random location and spread them out over my map.

srand(unsigned(time(NULL)));
vector<pf_particle_t> particles;
const int pf_amount = 100;

// Generate random particles around the area.
for (int p = 0; p < pf_amount; p++) {
      pf_particle_t particle;
      particle.pX = (double)rand()/(RAND_MAX)*(15)-7.5;
      particle.pY = (double)rand()/(RAND_MAX)*(8)-4;
      // particle.pYaw = (double)rand()/(RAND_MAX) * 2.0 * M_PI;
      particle.pYaw = 0.0;
      particle.pW = 0.0;

      particles.push_back(particle);
}

As you can see I used a vector that holds all my particles. It’s really easy to use it, especially iterate over it. On line three you see that I multiply with 15 and then subtract with 7.5. My map has a dimension of 15 but the robot is placed on the origin of the map, this makes sure my particles are randomly generated in the map and not outside the map. Next I should also grab all the locations of the fiducials (landmarks). You can do this hard-coded (look at your world file) or you can request them in your code:

// Get all the fiducials from the Sim proxy
for (int f = 0; f < 6; f++) {
      double x, y, a;
      sprintf(fidBuff, "Fid%d", f+1);
      simProxy.GetPose2d(fidBuff, x, y, a);
      FiducialObjects[f][0] = f;
      FiducialObjects[f][1] = x;
      FiducialObjects[f][2] = y;
}

If you simulate in stage you can add the SimulationProxy and so request various properties of the objects in your world file. A little trick, you can also add the Graphics3dProxy to draw your particles. This code should be in your main loop (while/for) and draws all the particles, again I used a vector to hold all the points to draw:

draw3D.Clear();
pts.clear();
// Now draw all the particles.
for(vector<pf_particle_t>::iterator pd = particles.begin(); pd != particles.end(); ++pd) {
	player_point_3d_t tmp_pnt;
	tmp_pnt.px = pd->pX;
	tmp_pnt.py = pd->pY;
	tmp_pnt.pz = 1;
	pts.push_back(tmp_pnt);
      }
draw3D.Color(green);
draw3D.Draw(PLAYER_DRAW_POINTS, &pts[0], pf_amount);

The Particle Filter code

Good now we the particle filter code. The idea is to measure the distance from the robot to the detected landmarks. Then measure the distance from the particles to the landmarks and apply a weight based on the correctness of those measurements. Finally re-sample the particles so that particles with lower weights are less likely to reappear in our new particle set.

robot measurements

First we request the position of the robot (using the Position2dProxy, note: in the configuration the robot should be set on odom for localization not gps!) and then we get the location of the fiducials and calculate the distance between the two coordinates.

// Measurement Model pre calculations
double Z[6];
// Check if we found any fiducial at all
if(fidProxy.GetCount() > 0) {
	// Loop through the detected fiducials
	for(uint32_t i = 0; i < fidProxy.GetCount(); i++) {
	  // Grap the fiducial
	  player_fiducial_item_t fiducial = fidProxy[i];
	  // Calculate the distance from the robot (odom pos) and fiducial object (static)
	  double fdist = DistBetween(p2dProxy.GetXPos(), p2dProxy.GetYPos(), FiducialObjects[fiducial.id][1], FiducialObjects[fiducial.id][2]);
	  // Add it to the measurment array Z
	  Z[i] = fdist;
	}

}

 Apply motion and measurement model

Now we apply the same motion as the robot on the particles. First we keep track of our previous location and our current location and find the distance between them. Because every particle can be in another heading direction we also have to include this in our motion model. We add random Gaussian noise, a popular algorithm is the Box-Muller algorithm:

This accounts for small possible errors and spreads our particles. Finally we apply the motion of the robot on the particles, lines 1-17 in the following code:

// Variable used for max weight
double mw = 0.0;
// We apply the same forward and turnspeed that we gave the robot to all the particles
for(vector<pf_particle_t>::iterator pd = particles.begin(); pd != particles.end(); ++pd) {
	/// Motion Model
	// Calculate traveld distance
	double dist = DistBetween(oldX, oldY, newX, newY) + RandomGauss(0.0, forward_noise);
	// Reset the angle for every particle, else we keep counting it up.
	double oriantation = 0.0;

	// How much did we turn, make note of the starting angel
	oriantation = pd->pYaw + (newYaw - oldYaw) + RandomGauss(0.0, turn_noise);
	// make sure we are still in the 0-2*pi range
	oriantation = fmod(oriantation,2*M_PI);

	// apply the calculated motion
	pd->pX += (cos(oriantation) * dist);
	pd->pY += (sin(oriantation) * dist);
	pd->pYaw = oriantation;

	/// Measurement Model
	// Set the probability to one and the distribution to 0
	double prob = 1.0;
	double pdist = 0.0;
	// Factor in the measurement and update the weight
	if(fidProxy.GetCount() > 0) {
	  for(uint32_t f = 0; f < fidProxy.GetCount(); f++) {
	    player_fiducial_item_t fiducial = fidProxy[f];
	    // Calculate the distance from every particle to the fiducials found
	    pdist = DistBetween(pd->pX, pd->pY, FiducialObjects[fiducial.id][1], FiducialObjects[fiducial.id][2]) + RandomGauss(0.0, sense_noise);
	    // Calculate the weight for every particle, noise of 5.0 (maybe less?)
	    prob *= Gaussian(pdist, 1.0, Z[f]);

	  }
	}
	// Save the highest variable
	if(mw < prob) mw = prob;

		      pd->pW = prob;

}

After that we need to add a weight to every particle. So we go over every fiducial found, calculate the distance from the fiducial to the particle and then calculate the Gaussian distribution. We also keep track of the highest weight, we need this for the re-sampling.

Resampling

So that movie explains the basic concept of this re-sampling step. Re-sampling can be done in multiple ways, this approach is the know as the Roulette wheel and as a complexity of O(n log n), not that good but its proven to better for localization.

/// RESAMPLE
int index = static_cast((double)rand()/(RAND_MAX) * pf_amount);
double beta = 0.0;
vector<pf_particle_t> particles_new;

for(vector<pf_particle_t>::iterator pd = particles.begin(); pd != particles.end(); ++pd) {
	beta += ((double)rand()/(RAND_MAX)) * 2.0 * mw;

	while(beta > particles[index].pW) {  
	  beta -= particles[index].pW;
	  index = (index + 1) % pf_amount;
	}
	particles_new.push_back(particles[index]);
}
particles.clear();
particles.assign(particles_new.begin(), particles_new.end());

The Result

My first experiment with a moving gif, better than another particle filter video on YouTube I guess.

Little animated gif shows the robot and particle filter

Next is to see how I can remove particles that are invalid (they went through a wall), use the laser measurements instead of fixed landmarks and other improvements.

The post Particle Filter Implementation in Player/Stage appeared first on Fritz-Hut.

The goal of the project

The project overview

The idea is illustrated in the following image. A server is used and utilises a self-learning algorithm that learns preferences of the user. The user has a smart phone and can adjust the environment, these adjustments are registered on the server and used in the self-learning algorithm.

So a little example, I walk in the room and the light turns on, I find it to bright so change it to 70% from the original 100%. I do this every day for 3 days and on day 4 the light switches on, but instead of the 100% illumination, it is only on for 70%. The system learned my preference and applied it.

The smart phone communicates with the server using WiFi and the server must communicate with the application (light bulb, oven, water heater  lamp, light strip …) using DASH7 technology.

We were free to choice the applications and the mobile application (dedicated, web-based universal or hybrid). There were only 8 days left and we were allowed to take short cuts (aka dirty code and tricks) to get a result.

DASH7 Technology

Everyone has an idea how a smart phone application might look. But the DASH7 part is a big unknown. There are developer kits from WizziLab for rather a hefty price (€100). A PhD student at my University made a smaller tag that only costs around €10, or even less.

Combine those two items and you can program the DASH7 tags. Right now the DASH7 Alliance is building a complete OSI stack for DASH7 and writing the code with the help of researchers/PhD students. They already finished the Physical and Data-Link Layer, we programmed using this stack on the Data-Link Layer.

This also means that we had to program embedded devices, luckily we could use C code and the library from the DASH7 Alliance include most of the important commands to send receive and handle interrupts. Quite cool using cutting-edge technology.

Task Assignment

So we (me and 6 colleagues) started to draft out a general concept/flowchart so we could divide this project in subsections. Every subsection is developed by one person who is responsible. We came up with the following flowchart.

Our basic flowchart, every colour represents a different person.

My job was to program the DASH7 tags and make sure the applications worked. Another student is on the same subsection as me but his task was to wire electronics on the GPIO pins of the DASH7 tags. I’ll only talk about my part because I don’t really know all the details.

DASH7 Application

We picked the following application. We have a house with 2 rooms, a living room with a TV and a workroom with a desk and a computer. When the users walks to the TV it should automatically play a video, if he walks away the video should pause. When the user enters the workroom the laptop should automatically open certain applications. Whenever the user leaves or enters the room the light should switch on or off accordingly.

The DASH7 application with two rooms

A couple of things come together here. Firstly, we have an approximation application, meaning that if a user comes near a tag something should happen. Secondly, we defined three types of DASH7 tags, a server tag, application tag and mobile tag. Finally, we interact with 230v light bulbs using the 3.3v GPIO from the DASH7 tag.

Communications in the DASH7 application

A little note, I made this communication protocol because its easy dirty and the way to go for our little application. In no way this is useful if more than 3 tags are used and way to much overhead comes in the mind. Because of the time constrains I picked this easy, dirty solution.

Commands

Every command/message is send using the following protocol that I also used in my ArduinoPi code with one big difference every command is send in one byte (0×00-0xFF). So small recap, every command starts with @ and ends with :, the first number indicates the command and the second the value. I also included the comma between the command and value but it’s not really needed since every value is only one byte. The reason was that later on I could always change it back to dynamically allocate the command and value.

Communications

So the communication is based on the following image (yes I love images).

Communications in the DASH7 application

Dotted lines indicated wireless communication and solid lines include wired communication. It’s quite simple, every command has its own value followed by the value (if any). The mobile tag always sends out a one every second, the user wears this tag (can be a watch or small keychain).

The application tag receives a one, pulls out the dbm value and sends it back as command 2 to the server tag. The server tag sends the dbm to the server over a serial connection and the server decides if the user is close enough to switch the light. Note that one short-cut is included, the application tag will also send the dbm value to the computer who will start the video if the user is close enough, normally the server should decide this (for various reasons we didn’t include this).

The server tag also receives a one and sends his dbm value to the server, which decides if the user is close enough to switch the lamp or start-up the applications on the computer.

Other communication is also included because we had to read out a sensor (light sensor), I used the same way of communication for the two light sensors.

Result

The following video demonstrates our project. We failed to include the smart phone (HTML5 based jQuery mobile application) because we were unable to connect it with the server/DASH7 tags. We used a WCF Service Library and apparently it is unable to start, talk or do anything when it receives commands from the smart phone. We only figured this out on the last day and try to convert it to a WCF Service but we had no more time and decided to remove it, shame really.

Conclusion

It was a fun project and I’m pleased with the results. We already had to present this project and the promoters/teachers were quite pleased but had some comments, mainly about the extendibility of our project.

The post Home Automation using DASH7 appeared first on Fritz-Hut.

A new year and for me a time to reflect on my blog and I’m quite happy with the result. General I made one post a week (sometimes even more) and talked about various topics.

For 2013 I plan to blog actively on my Master Thesis. The next three weeks will be quite busy (or not) I still have 3 examinations to finish (why o why) that only count for 30% of the total marks for every course. The other 70% I already acquired during the year by making projects.

There is one blog post in process (for a while now) about DASH7 Home Automation, a fast prototype (8 days) project.

The post Happy Holidays appeared first on Fritz-Hut.

Augmented Reality

First you should host the application using XAMPP (Windows) or Apache and you also should use the latest chrome browser. I’ve never tested this on Firefox (don’t even bother trying to use IE or Safari). If you have a working apache/xampp installation then host the applications by copying all the files in the following archive in your www or htdocs folder.

Now surf to localhost and accept the use of the webcam. Normally you should see that your browser detects and uses your webcam. If this fails then try another webcam. In the archive I also included a word file with the markers you should print before using the application.

Pattern Recognition

Before you can use the following code you should make sure you installed OpenCV, libconfig, ARToolKit and ARToolKitPlus. I don’t plan on giving out my complete Visual Studio project (the project is originally from somebody else), but my basic commands and the most important function (classify) is included. This ensures that lazy-copy-cats will fail and people looking for information will succeed.

// The gaussian distrubution of the features calculated
float cLike(float val, int feature) {
	return (1/sqrt(2*M_PI*(pow(circle_parm[feature][1], 2))))*exp((pow(val-circle_parm[feature][0], 2))/(-2*pow(circle_parm[feature][1], 2)));
}

float nLike(float val, int feature) {
	return (1/sqrt(2*M_PI*(pow(noise_parm[feature][1], 2))))*exp((pow(val-noise_parm[feature][0], 2))/(-2*pow(noise_parm[feature][1], 2)));
}

float aLike(float val, int feature) {
	return (1/sqrt(2*M_PI*(pow(animal_parm[feature][1], 2))))*exp((pow(val-animal_parm[feature][0], 2))/(-2*pow(animal_parm[feature][1], 2)));
}

// This is the main classify class, it will threshold the marker, add some transformations
// and then also classify it
void classify(ARToolKitPlus::ARMarkerInfo* markerinfo, IplImage* img, int i) {
	// Only continue if area is big enough
	if(markerinfo->area < 1000) return;
	// First we must clear all temp data, just to prevent memory access issues
	cvZero(contMarkImage);
	cvZero(minCircleImage);
	cvZero(filledImage);
	cvZero(maskImage);
	cvZero(trainingset);

	// Warp the marker so that we obtain a square (undo the perspective transformation)
	// The resulting marker image can be used for classification
	IplImage* detectMarkImage = unWarp(img, markerinfo);
	cvSetImageROI(detectMarkImage, cvRect(markerinfo->pos[0]-50, markerinfo->pos[1]-50, markPixel, markPixel));
	cvShowImage("Detected Marker", detectMarkImage);
	cvResetImageROI(detectMarkImage);
	//Threshold the marker. Convenient for border detection
	IplImage* thresMarkImage = threshold(detectMarkImage, markerinfo);
	cvShowImage("Thresholded Marker", thresMarkImage);
	// return if the dimensions aren't correct, happens alot => cause of random crashes
	if(thresMarkImage->width != thresMarkImage->height) return;
	// Now we can copy everything
	cvCopy(thresMarkImage, trainingset);

	// Lets calculate a histogram and count the black and white pixels
	cvCalcHist(&thresMarkImage, histogram);
	float black = (float)*cvGetHistValue_1D(histogram, 0);
	float white = (float)*cvGetHistValue_1D(histogram, 1);
	float whiteBlackRatio = white / black;

	// There must be some white pixels, but markers arent more then half white, so we can check that
	if(whiteBlackRatio < 0.05 && whiteBlackRatio > 1) return;

	// Create a cross structur element 4x4 with a center of 2, 2
	IplConvKernel* crossStruct = cvCreateStructuringElementEx(4, 4, 2, 2, MORPH_CROSS);

	// Apply a closing and do it 3 times with the cross structure
	cvMorphologyEx(thresMarkImage, thresMarkImage, thresMarkImage, crossStruct, MORPH_CLOSE, 3);

	// Now expand the image with a border, attempt to remove the white small lines from the marker
	cvCopyMakeBorder(thresMarkImage, maskImage, cvPoint(border,border), 0);
	// Copy the image
	cvCopy(maskImage, filledImage);
	// Now apply the floodFill, flip it and combine the image to fill the holes
	cvFloodFill(maskImage, cvPoint(0,0), cvScalar(255));
	cvSubRS(maskImage, cvScalar(255), maskImage);
	cvAdd(maskImage, filledImage, filledImage);
	cvShowImage("Filled Marker", filledImage);

	// Some values for the parameters
	CvSeq* contour = 0;
	float areaTot = 0.0, lengthTot = 0.0, metric_cTot = 0.0, count = 0;
	float area = 0.0, areaMax = 0.0, metric_c = 0.0, metric_s = 0.0, metric_sTot = 0.0, length = 0.0, lengthMax = 0.0;

	cvZero(contMarkImage);
	CvMemStorage* storage = cvCreateMemStorage(0);
	// Find contours
	cvFindContours(filledImage, storage, &contour, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

	// Contour total doesn't work at all, strange, lets count it ourself
	for(; contour != 0; contour = contour->h_next) {
		// Also draw contours (so pritty)
		cvDrawContours(contMarkImage, contour, cvScalar(255), cvScalar(255), 0, 0, 8);
		// Calculate all the different features
		area = fabs(cvContourArea(contour, CV_WHOLE_SEQ));
		length = fabs(cvArcLength(contour, CV_WHOLE_SEQ));
		areaTot += area;
		lengthTot += length;

		// make sure we fetch the biggest area and length
		if(area > areaMax) areaMax = area;
		if(length > lengthMax) lengthMax = length;
		// Also count the amount of contours we have
		count++;
	}
	// If the total area is bigger then 8000 we clearly don't have a marker
	if(areaTot > 8000) return;
	cvShowImage("Contour Function", contMarkImage);
	// The values will hold the probabilities for every class
	float cFeatures = 0.0, nFeatures = 0.0, aFeatures = 0.0;
	// Only continue if the values are above zero
	if(areaTot > 0 && lengthTot > 0) {
		// Calculate the circularity and squareness
		metric_c = areaMax * 4 * M_PI / pow(lengthMax, 2);
		metric_s = areaMax * 16 / pow(lengthMax, 2);
		metric_cTot = areaTot * 4 * M_PI / pow(lengthTot, 2);
		metric_sTot = areaTot * 16 / pow(lengthTot, 2);

		if(!trainingset_bool) {
			// Naive bayesiaanse classifier, using the blob count, circularity and black and white ratio
			cFeatures = cLike(metric_cTot, 0) * cLike(count, 1) * cLike(whiteBlackRatio, 2);
			nFeatures = nLike(metric_cTot, 0) * nLike(count, 1) * nLike(whiteBlackRatio, 2);
			aFeatures = aLike(metric_cTot, 0) * aLike(count, 1) * aLike(whiteBlackRatio, 2);

			// Now check for the highest probability, and then save the class name in the type variable
			if(cFeatures > nFeatures && cFeatures > aFeatures) {
				sprintf(type, "Circle");
			} else if(nFeatures > cFeatures && nFeatures > aFeatures) {
				sprintf(type, "Noise");
			} else {
				sprintf(type, "Animal");
			}
		}
	}

	// setup the OpenGL transformation
	updateTransformationMatrix(markerinfo);
	glMatrixMode(GL_MODELVIEW);
	glLoadMatrixf(gl_para);
	// Switch the color depending on the number of markers in screen
	glColor3f(color[i%5][0], color[i%5][1], color[i%5][2]);
	glLineWidth(1.0f);
	glBegin(GL_LINE_LOOP);
	glVertex2f(-40.0f, 40.0f);
	glVertex2f(40.0f, 40.0f);
	glVertex2f(40.0f, -40.0f);
	glVertex2f(-40.0f, -40.0f);
	glEnd();
	// Now print the text on the screen
	sprintf(type_string, "Type: %s", type);
	renderString(markerinfo->pos[0]-40, markerinfo->pos[1]+60, GLUT_BITMAP_HELVETICA_18, type_string);

	// Check the configuration, if we have to save data for feature selection or confusion matrix
	if(trainingset_bool) {
		float data[] = {count, whiteBlackRatio, markerinfo->area, lengthTot, areaTot, metric_cTot, metric_sTot, areaMax, lengthMax, metric_c, metric_s};
		int data_size = (sizeof data)/sizeof(data[0]);
		saveData(trainingset, data, data_size);
	} else if(predictclass_bool) {
		float data[] = {count, whiteBlackRatio, markerinfo->area, lengthTot, areaTot, metric_cTot};
		int data_size = (sizeof data)/sizeof(data[0]);
		saveData(trainingset, data, data_size, type);
	}

	cvClearMemStorage(storage);
	cvReleaseMemStorage(&storage);
	cvReleaseStructuringElement(&crossStruct);

}

The post Augmented Reality & Pattern Recognition Sources appeared first on Fritz-Hut.

My post is based on the following paper:

Fox, D., Burgard, W., Dellaert, F. & Thrun, S. Monte carlo localization: Efficient position estimation for mobile robots. Proceedings of the National Conference on Artificial Intelligence, 1999, pp. 343-349.

Monte-Carlo Localization

Localization can be done in a couple of ways. A popular method was a grid-based approach, without going into details, the main disadvantage was updating the grid on 80s-90s computers. Also some problems like robot kidnapping (robot is moved by the user) were impossible to solve with a grid-based approach.

Monte-Carlo Localization is based on Markov localization, the key concept is to represent the posterior belief by a set of weighted, random particles

A particle contains the following information it contains the robots position and heading and also a weighting factor that’s analogous to a discrete probability. A sample set represents an approximation of a probability distribution, which means that the sum of all the particles in the sample set should equal to 1. Monte-Carlo Localization works in two steps: Robot motion and sensor readings.

Step 1: Robot Motion

When the robot drives, new samples are generated that approximates the robot’s position after the motion. Each sample is generated randomly, by drawing a sample from the previous sample set with a likelihood that is determined by their -values.

Step 2: Sensor Readings

After the robot motion a sensor reading is done. When the robot sensors are available, the measurements are incorporated in the algorithm. The weight of every sample is recalculated, let be a sample then

is the new weight were is a normalizing factor that enforces a probability distribution, meaning it enforces that the sum of all the weights is 1.

And that’s it, very easy very random and it works. I know that my explanation may still confuse people so I found a YouTube video that explains particle filters with pictures and drawings, making it easier to understand. The video is explained by Sebastian Thrun himself.

The post Monte-Carlo Localization appeared first on Fritz-Hut.

Configuration of cmake

First a CMakeLists.txt file should be made in the main project directory. For example I used the following file from the documentation and changed it a bit.

CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
# Set your project name and version
PROJECT (playerstagetest)
SET(PROJECT_VERSION 0.0.1 CACHE STRING "CMake Example Project Version Number")

# Build flags for different configurations
# Use is determined by the option CMAKE_BUILD_TYPE
set(CMAKE_BUILD_TYPE "DEBUG")

set(CMAKE_C_FLAGS_DEBUG "-g -Wall ")
set(CMAKE_C_FLAGS_RELEASE "-O2 ")

set(CMAKE_CXX_FLAGS_DEBUG "-g -Wall ")
set(CMAKE_CXX_FLAGS_RELEASE "-O2 ")

# If Player isn't installed to /usr, you'll have to edit this path to point
# to the location of UserPlayerC++.cmake and UsePlayerPlugin.cmake
SET (CMAKE_MODULE_PATH "/usr/local/share/cmake/Modules")

# These files include macros that make building against Player easier.  They provide
# the PLAYER_ADD* macros
INCLUDE (UsePlayerC++)
INCLUDE (UsePlayerPlugin)

# This command adds an example PlayerC++ client called 'exampleclient', using
# the 'exampleclient.cc' source file.
PLAYER_ADD_PLAYERCPP_CLIENT (exampleclient SOURCES main.cpp)

# If you also want to include a Player plugin driver, use these directions:
# This command adds an example plugin driver which will be called 'libexampledriver.so'
# which uses the source file 'exampleplugin.cc'
# PLAYER_ADD_PLUGIN_DRIVER (exampledriver SOURCES exampleplugin.cc)

#add_executable(playerstagetest main.cpp)

Now copy the complete project DIR to the Raspberry Pi and then create a build directory. In the build directory run the following command:

mkdir build
cd build
cmake -DCMAKE_BUILD_TYPE=Debug /home/pi/PlayerProjects/PlayerStageTest/

The last value is the location of your PlayerStage Project. We only need to run this command once for the project, if we change our main.cpp we don’t need to run this command. Next we can run make to create our program.

make

The main advantage is that you can fully develop using Stage on your main computer and then export your source to the Raspberry Pi and run in there. One thing I don’t know if its possible to just run the player server and client at the same time. This means that you configure the player server for a specific robot and link it with the client file.

By changing the following line PlayerClient robot(“192.168.0.184″); I’m able to test out the player client on my Raspberry Pi and connect it with the player server running on my laptop.

So I’ve successfully installed Player on the Raspberry Pi. Now it’s waiting for the robot too arrive.

The post Player/Stage Compile on RaspberryPi appeared first on Fritz-Hut.

Installing Player (Player/Stage)

Player depends on the following packages that need to be installed using:

sudo apt-get install cmake g++ swig libboost-dev libboost-all-dev libltdl-dev libjpeg-dev libgstreamer0.10-dev libstatgrab-dev libusb-dev libasound2-dev libgsl0-dev libxmu-dev libxi-dev libfltk1.1-dev libopencv-contrib-dev libcv-dev freeglut3-dev

This will take very long, apparently all the packages are found (crazy). I don’t really know if I included too many dependencies (I’m not really sure about the opencv) but it wont hurt! First time I had a file corruption problem (selected wrong package), now everything seems ok. After updating all the packages, restart the Raspberry Pi!

Download the trunk of player from the Player/Stage svn:

wget http://playerstage.svn.sourceforge.net/viewvc/playerstage/code/player/trunk/?view=tar -O player.tar.gz

Decompress and expand the tarball, we also rename the output folder:

tar xzf player.tar.gz
mv trunk/ player/

Create a build folder

cd player/
mkdir build
cd build/

Great now its time to configure, we use the default configuration. Note that this can easily take 5-15min! Normally there shouldn’t be any errors (maybe some warnings):

cmake ../

Next we compile and install. Also note that the first commando can take around 1h-2h. It’s possible that you see a couple of warnings, nothing to worry about.

make
sudo make install
sudo ldconfig

Finally we test the installation using the Player help file. The following command should show the Player version number.

player -h

And we get the following result:

The post Installing Player (Player/Stage) on Raspberry Pi appeared first on Fritz-Hut.

Borenstein, J. & Koren, Y. The vector field histogram-fast obstacle avoidance for mobile robots Robotics and Automation, IEEE Transactions on, IEEE, 1991, Vol. 7(3), pp. 278-288

The original VFH algorithm is based on previous work on Virtual Force Field (VFF), a local path planning algorithm. Understanding VFH also requires a basic knowledge of the Virtual Force Field algorithm.

The Purpose of VFH

A robot should of course try to evade possible obstacles when driving around. A very basic approach can be used. For example lets look at our example robot. It has 3 ultrasonic range sensors mounted on the front side of the frame. The ultrasonic range sensors give back a distance measurement.

Simple algorithm would be, if the distance is lower than 0.5m then turn to other side with an angle of 60 degrees. Now the chances that the robot will drive in circles is pretty high. Also U-shaped corners are a no go for our little robot.

Creating a complex algorithm is also possible, but then again its possible that your robot will keep driving in the wrong direction (for example, you have to go from A to B but there is an obstacle in the way). VFH allows very accurate obstacle avoidance (using a laser) and plans a route to a specific goal.

Virtual Force Field (VFF)

First a bit about VFF, it’s important to understand the basic of this algorithm before understanding Vector Field Histogram. There are 3 steps in the VFF algorithm.

Step 1

VFF utilises a 2D Cartesians histogram to represent the obstacles. This grid moves along with the robot and has a specific size (33×33 for example). Each range reading will update one cell in this histogram and so creates a probability distribution. Only the cell that lies straight in front of the sensor will be updated, the surrounded cells are left alone. This seems as a very easy way, for example the ranger sensor detects an object and increases the probability of the specific cell. The trick lies in continuously and rapidly sampling for each sensor while moving and thus creating multiple readings for a specific cell and its neighbours. This creates a histogram probability distribution in which high certainty values are obtained in cells close to the actual location of the obstacle.

Step 2

The next step is applying the potential field idea to the histogram. It’s a simple idea. Every occupied cell in the histogram has a specific virtual force that works against the robot (pushing it away from the obstacle), those little virtual forces are combined in one big vector called . The goal that we try to reach also has a specific virtual force that pulls the robot in the goal’s direction. This force is called .

Combining the two repulsive and attractive forces gives us the new moving direction. Simple as that, more information and more mathematics can be found in the original paper but this explains the basic idea behind VFF.

 Step 3

Step 1 and 2 are combined in a real-time algorithm. VFF introduces a new algorithm for updating the small grid cell. In 1991 processing power and memory were scarce to find.

Vector Field Histogram

Of course VFF has a couple of disadvantages that why VFF is used at the base of Vector Field Histogram. Mainly because VFF introduces a lot of data but wastes it by counting up all the different vectors in the grid cell. Hundred data points are reduces to only 2 values, the angle and magnitude of .

So Vector Field Histogram uses the same principle but reduces the information in two steps. There are three data levels in Vector Field Histogram:

1. The highest level contains the histogram that continuously gets updated with sensor information, this is the same as in the VFF algorithm.
2. A polar histogram is constructed by reducing the histogram (from level 1) around the momentary location of the robot.
3. The lowest level contains the output of the Vector Field Histogram algorithm, the direction and the speed the robot must travel.

The first data reduction step maps the active region of the histogram grid on the polar histogram. Like VFF an active window will follow the robot with a specific size (for example 32×32 grid cells). But now we divide the polar histogram in different sectors K. If different cells have a higher number then the polar obstacle density will be higher in those sectors. A figure explains way better than words in my opinion.

If we have the histogram its possible to decide an optimal heading direction. The regions with very low values are possible directions were no obstacle is found. Now we only need to select the right direction, this direction should somewhat lead to the desired target.

The red regions indicate directions we should not travel, the green regions indicate save traveling directions.

Of course I totally ignored any formulas, but this overview gives the basic idea of the VFH algorithm. It uses virtual force vectors to decide the optimal heading direction, using the  polar obstacle density histogram. VFH is better than VFF because it takes more data into account. For more information, a little google scholar search should lead to the complete paper.

Next time I’ll talk about Particle Filters!

The post Vector Field Histogram (VFH) appeared first on Fritz-Hut.

Reading papers on your subject is always a good start. But I believe that telling a story, writing a blog, is even a better way to prepare yourself. Trying to explain a paper or algorithm in human-understandable words is a challenge. Enough introduction, lets start with my subject.

The DASH7 work group (DASH7-Team)

My Master Thesis is part of the DASH7-Team, this team aims to develop an intelligent “house” (Smart House) based on the wireless sensor network called DASH7. DASH7 is basically a new wireless technology like Bluetooth or ZigBee, but mainly used to send small packets of data (not a movie or pictures) like sensor information (light on or off?).

The team consist of 5 people who are actively involved in creating the Smart House. Two other people are also busy with DASH7 technology but have a subject that has nothing to do with the Smart House.

The idea is that we (me and my 4 colleagues) create a Smart House that basically disables the need for any type of button, switch, knob,… This means that the Smart House will automatically switch on the radio, adjust the volume, change the temperature in the House, switch on lights,…

My Master Thesis

My subject is not that involved in the global concept of a Smart House. I have to program a robot that will display information to the user, a sort of Butler Robot (with no arms, so no fetching beers…). The information displayed can be emails, Twitter status, Facebook spam and information about your current Smart Home settings and profile. The robot must locate itself using DASH7 technology. This seems easy, sure drive around, locate yourself using DASH7 and done. Nope not really.

Imagine you need to drive to an address where your never been before, you use your GPS right? Good now let’s assume you don’t have a GPS, you only know your current location and the location were you need to go. Still thinking about leaving?

The solution to this problem is known as SLAM (Simultaneous Localization And Mapping). To know were you are you need a map, but to acquire a map you need to know were you are. This calls for an IMPOSSIBRU (know your internet meme’s).

My research focusses on acquiring the map. You can just draw a little floor plan, add this, and hope it will work. There are also other techniques for acquiring the map. After acquiring the map another part starts, how to avoid obstacles that are not on the map (MEOW!) and how to detect DASH7 tags that are placed all around the Smart House.

Whats coming up

In the next few weeks I’ll post more about my ongoing research and try to tackle some problems and technologies. I will mainly talk about robotics. Most of my information comes from papers I’ve read and studied. After the theory I’ll talk about my practical approach (robot, hardware, software,…).

The post Master Thesis Subject DASH7 appeared first on Fritz-Hut.