This component allow to record all the interfaces of the robot. Now is posible to record all the interface that provides gazeboserver or playerserver.

It's posible to configurate speed record, which interfaces do you want to record, where do you want to save the camera files, etc.

A configuration file example may be like this:

Recorder.FileName=datos.jde
Recorder.Encoders.Proxy=encoders1:tcp -h localhost -p 9999
Recorder.Encoders.bool=1
Recorder.Laser.Proxy=laser1:tcp -h localhost -p 9999
Recorder.Laser.bool=1
Recorder.Camera1.Proxy=cameraA:tcp -h localhost -p 9999
Recorder.Camera1.bool=1
Recorder.Camera2.Proxy=cameraB:tcp -h localhost -p 9999
Recorder.Camera2.bool=1
Recorder.PTEncoders1.Proxy=ptencoders1:tcp -h localhost -p 9999
Recorder.PTEncoders1.bool=0
Recorder.PTEncoders2.Proxy=ptencoders2:tcp -h localhost -p 9999
Recorder.PTEncoders2.bool=0
Recorder.Pose3DEncoders1.Proxy=pose3dencoders1:tcp -h localhost -p 9999
Recorder.Pose3DEncoders1.bool=1
Recorder.Pose3DEncoders2.Proxy=pose3dencoders2:tcp -h localhost -p 9999
Recorder.Pose3DEncoders2.bool=1
Recorder.Hostname=localhost
Recorder.Port=9999

Recorder.Laser.Samples=180
Recorder.Hz= 5

To execute this component:

./recorder --Ice.Config=recorder.cfg

Replayer

This componente allow to replay de information saved from record component.

It's posible to configurate the reproduction speed, which component do you want to reproduce, etc

#without registry
Replayer.Endpoints=default -h localhost -p 9999
#with registry
#Replayer.Endpoints=default
#Replayer.AdapterId=Replayer1
#Ice.Default.Locator=IceGrid/Locator:tcp -h localhost -p 12000

#Player server configuration
Replayer.Hostname=localhost
Replayer.Port=6665
Replayer.Initial_position.X=0 		#mm
Replayer.Initial_position.Y=0			#mm
Replayer.Initial_position.Theta=90	#deg
Replayer.Laser=1
Replayer.Laser.Num_readings=180
Replayer.Laser.Readings_per_degree=1
Replayer.Encoders=1
Replayer.Motors=1
Replayer.NCameras=2
Replayer.PTMotorsI=1
Replayer.PTMotorsII=1
Replayer.PTEncodersI=1
Replayer.PTEncodersII=1
Replayer.pose3dmotors1=1
Replayer.pose3dmotors2=1

#camera 1
Replayer.Camera.0.Name=cameraA
Replayer.Camera.0.ImageWidth=320
Replayer.Camera.0.ImageHeight=240
Replayer.Camera.0.Format=RGB8

#camera 2
Replayer.Camera.1.Name=cameraB
Replayer.Camera.1.ImageWidth=320
Replayer.Camera.1.ImageHeight=240
Replayer.Camera.1.Format=RGB8

Replayer.Hostname=localhost
Replayer.Port=9999

Replayer.Speed=1
Replayer.FileName=images
Replayer.Sensors=datos.jde

To execute this component:

./replayer --Ice.Config=replayer.cfg

KinectServer

This component provides a cloud point from Kinect. A configuration file example may be like this:

KinectServer.Endpoints=default -h localhost -p 9999

To execute this component:

./kinectServer --Ice.Config=kinectServer.cfg

Kinectviewer

This component visualize the cloud point which are served by the component kinectServer:

A configuration file example may be like this:

KinectViewer.Kinect1.Proxy=kinect1:tcp -h localhost -p 9999

To execute this component:

./Kinectviewer --Ice.Config=kinectviewer.cfg

Training Period

I am in the first phase of my project where I'll learn the methodology and tools by learning the platform (jderobot). Jderobot can be installed from Debian packages though I initially installed by compiling from zero.

I'll need to learn to program GUI in GTK, thera are a lot of (tutorial) on internet. I'll study the operation of the cameras, their parameters, geometry, ect.

My Fisrt Compenent

To learn how to use the platform jderobot5, I created my first component based on component (CameraView). Consists on display an image and contains one button that when clicked on it the program proceed to process the image with a (Canny filter).

This component is based on two components: (cameraserver) not be changed, this component handle video sources and (cameraView) provides an interface to display the video, this component is going to be the base for my first component, I have to apply a filter to the image with the help of (OpenCV)(Open Source Computer Vision) libraries, gives us programming functions for real time computer vision.

MyFirst Component	MyFirst Component (Canny filter)

Opencv Demo

To continue learning things about JdeRobot 5.0 and OpenCV libraries, I will create a new component based on a component of Jderobots 4.3, which aims to show the power of the OpenCV libraries. TThis module implements different images processing algorithm such as edge detection filters (Canny and Sobel), color filter in the HSV color space, the Hough transform (Standard and probabilistic) or motion detection by Optical Flow

We can see a snapshot of the GUI here:

There are more imformacion about the component that is in the official repository (here). This component has been created by (Ruben Barriada) and me.

Color Tuner

This component is based on a JdeRobot version 4.3, implements three color filters in a different color space: RGB, HSV and YUV.

(RGB) color space is based on three matrices containing information intensity of each color channel (red, green, blue). The components can take values 0-255 if we consider that the images are 8 bits per channel

(HSV) color space (Hue, Saturation, Value) is a nonlinear transformation of RGB color space. The components can take values H (0 º -180 º), S (0-1) and V (0-255).

(YUV) color space defined in terms of a luminance component and two chrominance components. this model is closer to human perception model than standard RGB. The components can take values Y (0 - 1) U (-0.436 to 0.436) and V (-0.615 to 0.615)

We can see two screenshot of the GUI :


colorTuner HSV	colorTuner YUV

In the program interface in the lower part the corresponding color space to HSV and YUV, we can click on the image for selecting the desired color filter, change it from directly using the sliders or by clicking on the color space.

Projecting Points in 3D

To continue to learn things about JdeRobot platform, this time my work will consist of projecting a 3D point, this work would consist of several stages. First, I have to know and understand the extrinsic and intrinsic parameters of the cameras, camera calibration using calibrartor module, and creating a module that represents a 3D world where you can select which point we want to project in 3D

Parameters of a camera

The intrinsic parameters

Depend on the model used for the representation of the camera. In the pinhole model:

<math>f_x </math> -> focal length factor multiplied by the pixel size on X axis

<math>f_y </math> -> focal length factor multiplied by the pixel size on Y axis

<math>(u_0, v_0) </math> = start point

Extrinsic parameters

Represent the position and orientation of the camera in the world. In general, presented with two translational and rotational matrices RT.

Calibrator

This point is very important, if not properly calibrated the cameras are not going to be posible to project points in 3D points correctly. To find the parameter, you must use the calibrator module, this section will learn how to launch and monitor its operation. The calibration of a camera is to make the world lines fit with the image objects.

The first is to launch gazebo:

gazebo /usr/local/share/gazebo/worlds/introrob.world

and gazeboserver, which is capable of communicating with gazebo using JdeRobot platform, you can retrieve information from various types of sensors: laser, motor, or in our case we are interested in the cameras.

cd /usr/local/share/jderobot/glade/
gazeboserver --Ice.Config=../conf/gazeboserver.cfg

Once launched these two components and we can launch the calibrator module.

calibrator --Ice.Config=calibrator.cfg

In the file calibrator.cfg you have to indicate where is the file of the world, with the paremeter Calibrator.World.File

An example of this file:

Calibrator.Camera.Proxy=cameraB:tcp -h 0.0.0.0 -p 9999
Calibrator.World.File=./config-example/calib-world

#Extrinsics, position
Calibrator.Config.Position.X=163
Calibrator.Config.Position.Y=115
Calibrator.Config.Position.Z=286
Calibrator.Config.Position.H=1.000000

#Extrinsics, orientation
Calibrator.Config.FOAPosition.X=896
Calibrator.Config.FOAPosition.Y=110
Calibrator.Config.FOAPosition.Z=286
Calibrator.Config.FOAPosition.H=1.000000

#Intrinsics
Calibrator.Config.Fx=277.0
Calibrator.Config.Fy=277.0
Calibrator.Config.Skew=0.0
Calibrator.Config.U0=120.0
Calibrator.Config.V0=160.0
Calibrator.Config.Columns=320
Calibrator.Config.Rows=240
Calibrator.Config.Roll=0.00

The format of the world-file is:

#comments
worldline x1 y1 z1 h1 x2 y2 z2 h2
worldline -5000,000000 2000,000000 0,000000 1,000000 -5000,000000 -2000,000000 0,000000 1,000000
...

It is very important that the world coordinates have the same references found that gazebo

Calibrate gazebo World

we must keep in mind that since we are calibrating a gazebo world, the intrinsic parameters of cameras are ideal. Therefore it follows that:

optical center --> <math> (u_0,v_0) = (\frac{height}{2},\frac{width}{2})</math>

focal distance--> <math> f=\frac{height}{2tan(\theta/2)}</math> where <math>\theta=60^{\circ}</math>

skew --> <math>0.0</math>

So we only use this module to calibrate the extrinsic parameters.

Creating a component

This component is based on introrob, show two cameras and a 3D world, when we click on some of the two images, the point becomes punctured a line in the 3D world, since the focus of the camera to a given distance.

Here you can see a short video demonstration.

Debugging Calibrator

This new task is going to be to modify the module "calibrator", this module is very important because it gives us the possibility to calibrate the cameras, I have to fix some bugs, KRT matrix display, create an output file with the parameters and also compatible with ICE and finally be able to load a file from a world in runtime.

Here you can see a snapshot of the new GUI:


KRT Matrix	Calibrator GUI

As you can see in the GUI, you have several options that you can select it in the differents buttons:

- Watch the optical center
- Save the parameters in a file compatible with ICE
- Load the lines of a world.

When you modify de value of the scrollbar, you can see a widget with the values of the matriz KRT, as you can see from the image above.

Here you can see a video:

Face detection

At this point we create a component to detect faces, so we've created a component based on CameraViewer. using opencv that provides much of the work of the detection of faces and a temporary motor at a rate of 10 fps.

The face detection algorithm is that of Viola & Jones, the algorithm smooths the image and move it to grayscale, since colors are not relevant, because the search will focus only on geometric shapes.

When you get a picture with the proper dimensions and colors are appropriate to the detection of faces in the picture. It uses the following function:

CvSeq * cvHaarDetectObjects (const IplImage * img, CvHidHaarClassifierCas-chain * cascade, CvMemStorage * storage, double scale factor, neighbors int min, int flags);

This function find rectangular regions in the input image that have a high probability of containing the objects for which the cascade parameter (the classifier) is trained by returning these regions as a sequence of rectangles. This function scans the image several times on different scales. Applying heuristic algorithms to reduce the number of regions examined.

So I just should be passed as a parameter CvHidHaarClassifierCascade trained to detect faces. These classifiers are XML files and in particular, has used a classifier that allows us to find OpenCV faces front. This classifier is haarcascade frontalfacealt.xml. Then get a sequence of rectangles where the sides so the only thing to do is go through this sequence and go circling each of the faces.

I have added a previous step to the algorithm, adding a color filter in the HSV color space, with the ability to enable or disable a button, to try to eliminate those areas that do not belong to the face.

Here you can see a video:

Playing cat and mouse

This task consist in play cat and mouse, we have to use two robots. I have divided the task into several steps:

Introduce two robots in gazebo
Creating configuration files
Running
Reactive algorithm

Introduce two robots in gazebo

To introduce two robot and gazebo has been necessary to look in the syntax of the configuration files from the worlds of gazebo, written in XML.

Here you can see a example of this file:


  <model:physical name="pioneer2dx_robot1">
    <xyz>0 0 0.145</xyz>
    <rpy>0.0 0.0 0.0</rpy>
    <collide>all</collide>


    <model:physical name="laser">
      <xyz>0 0 0.19</xyz>
      <attach>
        <parentBody>chassis_body</parentBody>
        <myBody>laser_body</myBody>
      </attach>
      <include embedded="true">
        <xi:include href="models/sicklms200.model" />
      </include>
    </model:physical>


<!-- left camera -->
    <model:physical name="sonyvid30_model_cameraA">
      <xyz>0.15 0.11 0.09</xyz>
      <attach>
        <parentBody>chassis_body</parentBody>
        <myBody>sonyvid30_body</myBody>
      </attach>
      <include embedded="true">
        <xi:include href="models/sonyvid30.model" />
      </include>
    </model:physical>

<!-- right camera -->
    <model:physical name="sonyvid30_model_cameraB">
      <xyz>0.15 -0.11 0.09</xyz>

      <attach>
        <parentBody>chassis_body</parentBody>
        <myBody>sonyvid30_body</myBody>
      </attach>

      <include embedded="true">
        <xi:include href="models/sonyvid30.model" />
      </include>
    </model:physical>

    <include embedded="true">
      <xi:include href="models/pioneer2dx.model" />
    </include>
  </model:physical>

  <!-- White Directional light -->
  <model:renderable name="directional_white1">
    <xyz>2.0 0 2.0</xyz>
    <enableGravity>false</enableGravity>

    <light>
      <type>spot</type>
      <spotCone>1000 1000 100</spotCone>
      <direction>-0.1 0 -0.9</direction>
      <diffuseColor>0.8 0.8 0.8</diffuseColor>
      <specularColor>0.1 0.1 0.1</specularColor>
      <range>10</range>

      <!-- Constant(0-1) Linear(0-1) Quadratic -->
      <attenuation>1.0 0.0 0</attenuation>
    </light>
  </model:renderable>

This info is only for the robot you need to include the ground plane or the light. I had some problem with the lighting of gazebo world, but changing the ground I managed to improve it.

It is important to know that when you add a new robot we have to rename the robot, the cameras and lighting. In this file the robot is called pioneer2dx_robot1 you can change the name to pioneer2dx_robot2 or whatever you want. The same with the cameras and lighting.

Creating configuration files

In this step we have to create different configuration files. First we must create the configuration files for gazeboserver (to access the cameras and the robot motors) and then for introrob.

At this point I found a bug in gazeboserver, you could not use a robot name that was not 'robot1' and with the camerasthe same we could only call 'cameraA' and 'cameraB'. It was necessary to correct the fault in the source code of gazeboserver.

The configuration files for the cat:

The file for gazeboserver catGazboserver.cfg

#without registry
GazeboServer.Endpoints=default -h localhost -p 9999
GazeboServer.ServerId=0
GazeboServer.ClientId=0
GazeboServer.RobotName=robot1

#cameras configuration
GazeboServer.NCameras=2

#camera 1
GazeboServer.Camera.0.Name=cameraA
GazeboServer.Camera.0.ImageWidth=320
GazeboServer.Camera.0.ImageHeight=240
GazeboServer.Camera.0.Format=RGB8

#camera 2
GazeboServer.Camera.1.Name=cameraB
GazeboServer.Camera.1.ImageWidth=320
GazeboServer.Camera.1.ImageHeight=240
GazeboServer.Camera.1.Format=RGB8

The RobotName have to be the same name the robot must have the same name as the configuration file gazebo. The same with the Name of the cameras.

The file for introrob catIntrorob.cfg :

Introrob.Motors.Proxy=motors1:tcp -h localhost -p 9999
Introrob.Camera1.Proxy=cameraA:tcp -h localhost -p 9999
Introrob.Camera2.Proxy=cameraB:tcp -h localhost -p 9999
Introrob.Encoders.Proxy=encoders1:tcp -h localhost -p 9999
Introrob.Laser.Proxy=laser1:tcp -h localhost -p 9999
Introrob.PTEncoders1.Proxy=ptencoders1:tcp -h localhost -p 9999
Introrob.PTEncoders2.Proxy=ptencoders2:tcp -h localhost -p 9999
Introrob.PTMotors1.Proxy=ptmotors1:tcp -h localhost -p 9999
Introrob.PTMotors2.Proxy=ptmotors2:tcp -h localhost -p 9999

In this file we have to indicate the name of correct cameras.

The configuration file for the mouse:

mouseGazeboServer.cfg

#without registry
GazeboServer.Endpoints=default -h localhost -p 9998
GazeboServer.ServerId=0
GazeboServer.ClientId=0
GazeboServer.RobotName=robot2

#cameras configuration
GazeboServer.NCameras=2

#camera 1
GazeboServer.Camera.0.Name=cameraC
GazeboServer.Camera.0.ImageWidth=320
GazeboServer.Camera.0.ImageHeight=240
GazeboServer.Camera.0.Format=RGB8

#camera 2
GazeboServer.Camera.1.Name=cameraD
GazeboServer.Camera.1.ImageWidth=320
GazeboServer.Camera.1.ImageHeight=240
GazeboServer.Camera.1.Format=RGB8

mouseIntrorbo.cfg

Introrob.Motors.Proxy=motors1:tcp -h localhost -p 9998
Introrob.Camera1.Proxy=cameraC:tcp -h localhost -p 9998
Introrob.Camera2.Proxy=cameraD:tcp -h localhost -p 9998
Introrob.Encoders.Proxy=encoders1:tcp -h localhost -p 9998
Introrob.Laser.Proxy=laser1:tcp -h localhost -p 9998
Introrob.PTEncoders1.Proxy=ptencoders1:tcp -h localhost -p 9998
Introrob.PTEncoders2.Proxy=ptencoders2:tcp -h localhost -p 9998
Introrob.PTMotors1.Proxy=ptmotors1:tcp -h localhost -p 9998
Introrob.PTMotors2.Proxy=ptmotors2:tcp -h localhost -p 9998

It very importan to indicate other port in the confiration file.

Running

To execute this you have to open 5 terminals. One for gazebo, two for gazeboserver and two others for introrob and control the robots.

Running Gazebo

gazebo catmouse.world

Running gazeboserver for the cat

gazeboserver --Ice.Config=catGazeboserver.cfg

Running gazeboserver for the mouse

gazeboserver --Ice.Config=mouseGazeboserver.cfg

Running introrob for the cat

introrob --Ice.Config=catIntrorob.cfg

Running introrob for the mouse

introrob --Ice.Config=mouseIntrorob.cfg

Reactive algorithm

To create the robot behavior has created an algorithm reagent.

We have implemented a color filter in the HSV color space to know where the robot and calculate their center of mass.
If the center of mass of the robot is out of a comfort zone marked with a green square in the image the robot will turn to find the other robot.
To know if we're close or far from the robot,we count the number of pixels it outputs the color filter. if it is larger than a threshold the robot stop and if it is smaller the robot will go forward.

Here you can see two snapshot:


Robot in the confort zone	Robot out the confort zone

Here you can see a video:

Playing with Kinect

On this occasion I will take the components that are in JDErobot to use the Kinect, jdenect and visornect. The Kinect provides different information: color image, the image of the IR and the image depth (11-bit gray scale).

Jdenect component provides the images and visornect component displays the images in color and depth, is able to move the neck, change the LED Kinect and a 3D world like we have in introrob.

It has taken the information of depth and color to draw in 3D so this shows the Kinect.

Here you can see a video:

Intersection Point

The aim here is to find the intersection point between two lines, which connect the cameras with focal centers. Again we used the component introrob

Here you can see a video:

Point Clould Library

Cloud Point Library (PCL) is a framework for working with point cloud, but what is a cloud of points?

It considered a point cloud to a series of data in a space of any number of dimensions that are scattered. Typically we speak of a cloud of points in 2D/3D spaces when referring to the data (depth, color, texture, shape ...) we obtain by means of sensors (cameras, lasers, sonar ...).

PCL makes available a series of algorithms to filter the point cloud and try to rebuild or recognition of objects from that cloud.

There are some tutorial in the official website. You can learn how to read or write cloud of points in an ASCII file, represent this cloud, use the library with Kinect...

If you want to install this library is currently support Ubuntu via PPA:

sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key 19274DEF
sudo echo "deb http://ppa.launchpad.net/v-launchpad-jochen-sprickerhof-de/pcl/ubuntu maverick main" >> /etc/apt/sources.list
sudo apt-get update
sudo apt-get install libpcl-dev

Here you can see a video:

PCL file format

The PCD file format is not meant to reinvent the wheel, beacuse PCD is not the first file type to support 3D point cloud data, but rather to complement existing file formats that for one reason or another did not/do not support some of the extensions that PCL brings to n-D point cloud processing. The computer graphics and computational geometry communities in particular, have created numerous formats to describe arbitrary polygons and point clouds acquired using laser scanners. Some of these formats include: PLY, STL, CAD, OBJ, X3D and many others.

The basic operaction with the point cloud data are read, write and concatenate.

Downsampling a PointCloud

Downsample, that is reduce the number of points, a point cloud dataset, using a voxelized grid approach.

In this example we have a cloud point of 307200 points and we reduce the cloud to 37766.

Here you can see a snapshot:

Planar model segmentation

we will learn how do a simple planar segmentation of a set of points, that is find all the points within a point cloud that support a planar model.

Here you can see a snapshot:

Extract indices

we will learn how to extract a set of indices given by a segmentation algorithm from a point cloud.

Here you can see a snapshot:

Cluster

we will learn how to extract Euclidean clusters.

Here you can see a snapshot:

Construct a convex hull polygon for a planar model

we will learn how to calculate a simple 2D convex hull polygon for a set of points supported by a plane.

Here you can see a snapshot:

Removing outlier

we will learn how to remove noisy measurements, e.g. outliers, from a point cloud dataset using statistical analysis techniques.

Here you can see a snapshot:

Surface triangulation

Here you can see how to run a greedy surface triangulation algorithm on a PointCloud with normals, to obtain a triangle mesh based on projections of the local neighborhoods.

Here you can see a snapshot:

Pass Through

we will learn how to perform a simple filtering along a specified dimension.

Here you can see a snapshot:

Getting Started

It is time to focus on my project and find a theme to develop and investigate. The theme was "Navigation with gestures Kinect and interpretation"

Locating a person with a point cloud

This time, we will use a point cloud, and explore ways to locate a person in 3D, for this I will study life functions.

The first thing we do is see how the population is distributed points. Observing the X and Y dimensions and despising the Z dimension, we use a prism to count the number of points that fall on it.

If the chosen prism is large, there is little resolution and is will be dificult to get information as you can see below:


Small resolution	Surface for small resolution

If the chosen prims is correct, it will be easier to get information the informtion:


Big resolution	Surface for big resolution

As you can see the peaks of the graph match with areas where there is greater concentration of points.

Other posibility is to change the size of the prism. The following graph corresponds to a big size of prism:

Health function

To create my "Health function" I have to start with some criteria. It has chosen to do with two prisms, one inside and one outside. The inside prism will be the same as used to calculate the population. and the outside will be slightly larger. To locate a person have to be aware that if the interior prism has many points and the exterior prism have bit, then it could be a person.

It is necessary to combine both functions robustly to achieve good results, one posible combination could be:

Health function --> <math> Health = InteriorPrism*factor + ExteriorPrism*factor</math>


Exterior prism	Inside prism

As can be seen in the graphs above, the graph that contains the data of the inside prism has a relatives maximum in the walls and the person. The other graphs, the graph that contains the data of the exterior prism has few points next to the person and the walls.

Here you can see a combination of this two functions:

Again has several relative maximum but not in the right place.

Gesture recognition

For this first test of gesture recognition is going to use as driver OpenNI (Open Natural Interaction),The main purpose of OpenNI is to form a standard API that enables communication with both: Vision and audio sensors and Vision and audio perception middleware, that soon there will be a component with this driver, because it provides a lower load in the system [1] and NITE( The PrimeSensor NITE Middleware is the perception component of the solution. ) The solution includes a sensor component, which observes the scene (users and their surroundings), and a perception component, or brain, which comprehends the user interaction within these surroundings.

Once installed Nite try one of the examples that can be useful to use is tracking bodies.

I created a small interface to display the tracking of a person, and recognize some gestures. Arms forming an L to the left or right and straight. Here you can see a small video:

Gesture recognition and tracking in 3D

Once we have the 2D tracking, thanks to the information we have about the depth, we can represent 3D points and display with OpenGL, the following two videos are shown a video front and another in the side to see the depth. The pink point is the center of mass of the person.


Front	Side

Teleoperator

Based on the gesture recognizer I have created a teleoperator to control the robot with gestures.With the arms extended the robot moves foward or With one arm extended and another over the head the robot turns to one side. As you can see in the video below:

Real Pionner

One of the major goals is to get all I'm developing for my PFC function with a real robot, the real robot is the same I use in gazebo. To use it you have taken the following steps. After several failed attempts to connect all the components at once, we have proceeded to connect the devices slowly. In this post we will see how to connect a laser and motor base.

Player

First launched player with the following configuration file.

Launched player

  >> player pionner.cfg

An example of configuration file: "pionner.cfg"

driver
(
  name "p2os"
  provides ["odometry:::position2d:0"
	    "power:0"
	    "bumper:0"
        "sonar:0"
           ]
  port "/dev/ttyUSB0"
)

driver
(
  name "urglaser"
  provides ["laser:0"]
  port "/dev/ttyACM0"
  pose [0.0 0.0 0.0]
  baud "19200"
  min_angle -90.0
  max_angle 90.0
  alwayson 1
)

Be careful and make sure the USB correspond to /dev/ACMX or ttyUSBX, where X is a number. With the command "dmesg" we can discover which it applies.

  >> dmesg

PlayerServer

Next we have to launch the playerserver component, this component is able to connect the components created by player and communicate with them.

playerserver playerserver.cfg

An example of configuration file: "playerserver.cfg"


PlayerServer.Endpoints=default -h localhost -p 9998

#Player server configuration
PlayerServer.Hostname=localhost
PlayerServer.Port=6665
PlayerServer.Initial_position.X=0 		#mm
PlayerServer.Initial_position.Y=0			#mm
PlayerServer.Initial_position.Theta=90	#deg
PlayerServer.Laser=1
PlayerServer.Laser.Num_readings=180
PlayerServer.Laser.Readings_per_degree=1
#PlayerServer.Sonars=0
PlayerServer.Encoders=0
PlayerServer.Motors=1
#PlayerServer.Bumpers=0
#PlayerServer.PTMotors=0
#PlayerServer.PTEncoders=0

Introrob

For this example has been modified introrob component, with this component only control the laser and the motor base of the robot.

Here you can see a small video with the robot moving:

Virtual Force Field (VFF)

VFF

To make the robot move in a logical way to the target, the robot must have an algorithm of motion. This time it has implemented Virtual Force Field.

VFF consists in calculating two vectors. The first vector, the attraction vector is generated toward the target and the second vector, the repulsion vector is generated based on the sensors in this case the laser and is opposed to the obstacles. To find the resulting vector you have to add this two vectors and the solution is used to move the robot.

As you can see in the picture below, this algorithm always tries to make the robot travel in the center of the way avoiding the obtacles.


VFF	VFF video

Detecting plane

I'm going to use the laser information to create a visual memory. The first thing to do is create more randoms points to the laser in the "z" axis to create "imaginary walls".

To detect the imaginary walls I had used RANSAC ( It is an iterative method to estimate parameters of a mathematical model from a set of observed data which contains outliers).

As you can see in the image below I had created a visual memory based on the laser. I had saved 50 readings of the laser.

Distance to the plane

When I have define de plane I have to calculate the distante of the laser points to the plane. In the picture below I draw 3 differente colors.

Yellow: Points who are in the plane or very near.
Blue: Point near the plane.
Pink: Point far the plane

If you see the image below you can see the "imaginary walls", in the image above you can see only the reading of the laser.

Patch plane

A pacht plane is a plane defined by points that are very close to the plane, in the top image the yellow points. To draw the polygon I had used Convex Hull thath is the convex envelope for a set of points. In the image below you can see my first patch plane.

Visual Memory based on patch plane

To implement my visual memory based on patch plane I have create a structure with the coefficients of the plane, the points of the polygon, the color and the maximun and minimun of the plane's points.

RANSAC problems

Sometimes RANSAC give some plane that are not real planes or this kind of plane it not interesting for me. To solve this problems I have use Cluster extraction. If the cluster algorithm give me only one cluster I decide that this plane is interesting for the visual memory and I add this plane to the memory but if the cluster algorithm give me more than one this plane it not interesting for the visual memory and I dicard this plane.


Good plane	Bad Plane

Dimensioned Plane

A problem that I find with the pacth plane is that It was defined as infinite planes and if you had another wall or a door you can mistake with the same plane and this is an error. To solve this problem is necessary to find the limits of the plane. I had used the maximun and minimun of the "X" and "Y" axis.

As you can see in the image below when the robot do a lap the visual memory can rebuild the walls of the circuit.


Circuit	Visual Memory

Improve Dimensioned planes

As you can see in the video above there are a lot of planes, there are some planes that are the same. I improved the algorithm to reduce the number of planes and get planes more representative.

Improve Dimensioned planes with life

In this points I had implemented a simple life function. When I discover a plane a get it a life If this plane is visited his life increases If the robot don't see the plane his life decreases. If the life is 0 or less I remove the plane from the memory.

Join Plane-Plane

This algorithm join planes with planes. If a plane is near other plane and is in the same plane, this mean that is the same plane, I join this two plane only in one more significative.

Join Points-Plane

When the laser detect new points first we try to join this points with one of the plane in the memory If this is not posible the algorithm to detect new planes start.

Add Kinect to the robot's odometry

In this point, I'm going to add the kinect to the robot's odometry. Then I'm goint to extract pacth planes too.

Kinect Patch Plane With real pionner

Now I'm going to use the same algorithm that I had used to get pacth plane with the laser with Kinect. This wall are real is the information that kinect provides. The only treatment to the point cloud has been downsampling. Because Kinect provides many points and makes the algorithm slow.


Video Corner	Corner

Kinect Patch Plane With real pionner in a corridor

To make easy the extraction of the planes, I'm going to move the pionner through a corridor. Here I had found a problem. Kinect generated walls in front of pionner that don't exists.


Through the corridor	Through the corridor in both directions

Introducing coherence

In this point I have to implement a coherence system to delete the planes that their position are not logical. For example as you can see in the picture below one of this planes are incorrect, the reasons are: If detect the plane who is near to the robot is imposible to detect the other one because you have an obstacle in the middle or Imagin that you recieve a bad read of the sensor and appear a plane in a random place. This reasons make necessary a coherence system.

To implement this algorithm, I have to proyect a straight(In the picture above you can see this lines), to the middle of the plane. When I had defined the straight and the plane is necessaty to solve the system. Remember that you can represent a straight with two planes. I had solve this problem using this method(In the picture above you can see the solution of the system in green).

A plane can also be represented by the equation:

where all points (x,y,z) lie on the plane. Substituting in the equation of the line through points P1 (x1,y1,z1) and P2 (x2,y2,z2)

gives:

Solving for u:

The denominator is 0 then the normal to the plane is perpendicular to the line. Thus the line is either parallel to the plane and there are no solutions or the line is on the plane in which case are infinite solutions

If it is necessary to determine the intersection of the line segment between P1 and P2 then just check that u is between 0 and 1.

Reference System

we have an absolute refence system of the world. The robot have is system reference too, the same for the Kinect. If we want to convert the reference of a point from one frame to another, we just have to apply the transformation given by a 4x4 matrix called the homogeneous matrix and defined by a rotation and a translation.

The matrix to transform absolute reference system to the robot reference:

<math>  RT = \left| \begin{array}{cccc}

cos(Robot_{\theta}) & -sin(Robot_{\theta}) & 0 & Robot_X \\ sin(Robot_{\theta}) & cos(Robot_{\theta}) & 0 & Robot_Y \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\

\end{array} \right| </math>

The matrix to transform the robot reference system to the camera refence system :

<math>  RT = \left| \begin{array}{cccc}

1 & 0 & 0 & Extrinsics_X \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & Extrinsics_Z \\ 0 & 0 & 0 & 1 \\ \end{array} \right| </math>

The matrix to transform the camera refence system to change the Tilt :

<math>  RT = \left| \begin{array}{cccc}

cos(Robot_{\alpha}) & 0 & -sin(Robot_{\alpha}) & 0 \\ 0 & 1 & 0 & 0 \\ sin(Robot_{\alpha}) & 0 & cos(Robot_{\alpha}) & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} \right| </math>

Real Pionner with Kinect

Now It's time to test my algorithm with the real pionner connecting Kinect. The first thing that I had to do is to remove the wires, for this reason I had build a simple circuit to connect Kinect to the robot's battery. Here you can see the circuit.

Here you can see two photos of the robot with kinect and the laptop in the corridor:

The first approach hasn't been so bad. The wall of the corridor are well detected:

But I have very big problem with the robot odometry, when the robot start turn to either sides some wall start to appear in many places, as you can see in the next photo and video:

To avoid this problem I have to develop SLAM or something similar to correct the robot's position.

Ahcorde-pfc

Contents

Project Card

Jderobot collaboration grant

Improving FAQ

JDErobot 5.0

Components

Recorder

Replayer

KinectServer

Kinectviewer

Training Period

My Fisrt Compenent

Opencv Demo

Color Tuner

Projecting Points in 3D

Parameters of a camera

Calibrator

Calibrate gazebo World

Creating a component

Debugging Calibrator

Face detection

Playing cat and mouse

Introduce two robots in gazebo

Creating configuration files

Running

Reactive algorithm

Playing with Kinect

Intersection Point

Point Clould Library

PCL file format

Downsampling a PointCloud

Planar model segmentation

Extract indices

Cluster

Construct a convex hull polygon for a planar model

Removing outlier

Surface triangulation

Pass Through

Getting Started

Locating a person with a point cloud

Health function

Gesture recognition

Gesture recognition and tracking in 3D

Teleoperator

Real Pionner

Player

PlayerServer

Introrob

Virtual Force Field (VFF)

Detecting plane

Distance to the plane

Patch plane

Visual Memory based on patch plane

RANSAC problems

Dimensioned Plane

Improve Dimensioned planes

Improve Dimensioned planes with life

Join Plane-Plane

Join Points-Plane

Add Kinect to the robot's odometry

Kinect Patch Plane With real pionner

Kinect Patch Plane With real pionner in a corridor

Introducing coherence

Reference System

Real Pionner with Kinect

Views

Personal tools

main

docs

projects with jderobot

Search

Toolbox