DIY SLAM

Let's Make Robots
,
1

 

DIY SLAM

momo4_4_png.jpg

Proposal of MoMo 0.9 with WiiDAR


SLAM Matrix
Source Sensor Comments
DVD Burner Laser diode - 650 nm ??mW WiiMote $38

DVD Reader diode - 650 nm WiiMote

Focused IR LED - IR beam flashlight

Pros:

  • matched sensor - wiimote was built to find IR

Cons:

  • dissapation and lack of coherence affects range
Key-Chain Laser 650 nm 5 mW $10  WiiMote
Pros:
  • easily obtainable parts
  • very high FPS 33-100?
  • does not require direct connection to computer !
Cons:
  • quality of data?  Needs filter adjusting to get a reflected
    light ?
  • small field of view?
 Laser Distancing Probe using WiiMote

 Laser Tracking with filter removed from WiiMote
 
None
Webcam $30 / OpenCV
Filters
Threshold

Pros:

  • good/clean tracking with no ambient light
  • resolution 640x480
  • large field of view without needing to rotate camera
  • cheap

Cons:

  • difficult/impossible to track in normal
    daylight  conditions
  • frame rate  ~5 to ~10 FPS with filtering 
  • USB camera needs direct connection to computer

 

laser_threshold.png
None Webcam / OpenCV
Filters
GoodFeatures
LK Optical Track

Pros:

  • incredible highly detail focused images
  • potentially capture/ranging multiple points in a 2D plane for
    depth evaluation
  • potentially much more data available
  • more ambient light the better !

Cons:

  • has to rely on good feature selection
    i.e. can not "choose" points of interest
  • needs more processing to determine "solids"
  • some of the points "slide" producing inaccurate depth
  • frame rate  ~5 to ~10 FPS with filtering
  • USB camera needs direct connection to computer
  • do to mechanical issues was unable to create smooth
    movement

lkoptical.png


WIIDAR - LIDAR with a Wiimote

2010.12.29

 

 

I have changed the title from WiiDAR to DIY SLAM because I’m more interested in the resulting simultaneous localization and mapping (SLAM).  The WiiDAR hopefully will be a means to that end.  When I entered the webcam entry in the sensor matrix, an old nagging thought came back to me.  Which is, it’s frustrating that I am turning an exceptional passive sensor (webcam) into a marginal active one (webcam + laser - ambient light).  I will have to add some more passive entries.

I recently acquired a wiimote and blue-tooth pc usb dongle.  The idea being I might be able to create some form of simple LIDAR to do SLAM.  In the past I have several experiments involving laser pointers and webcams. I have also previously utilized functions from OpenCV to create virtual models of some interiors.

In order to get some range information, I will need to find out what is available from a wiimote.  There is a considerable amount of wii-hack information available.  One of the greatest resources is wiibrew.  They have many wii-hacking projects.  I was interested in getting data from my device in Java on Linux.  WiiBrew has created a listing of libraries to interface with the wiimote. 
  • wiiuse - this looks like a impressive and extensive C library that can work for more than one platform - I downloaded the source - I got the necessary parts to compile.  The SDL graphics will not compile on my system, but I don’t think I need it.
  • CWiid - this was easy for me to install since it is in the Fedora repos  - yum list *wii* - it connected and the example wminput worked.  This was the first library I looked at but the wiiuse looks more powerful and is cross-platform
  • WIIREMOTEJ - this initially looked promising, however - the README said you needed a JSR082 bluetooth implementation, which seemed problematic, and worse - it appears WiiRemoteJ is free but not open source, so I wont be using it
  • motej - Yay! Apache open source.  Nothing better, unfortunately… nothing is really there.  There is no user doc, and no explanation on how any of the classes would work.  There has to be some JNA or JNI interface, but its not mentioned.  Beyond bleeding edge !
  • WiiuseJ - sweet - I’ll was planning to have to do this, but someone has already done it (and probably better!) JNI access to wiiuse
  • wiimote-whiteboard - this looks like a nice & open source project - I downloaded it but it failed to connect to the bluetooth stack - another JSR082 problem probably

 

Downloaded WiiuseJ sample app.  I can see it comes with binaries (libwiiuse.so & libWiiuseJ.so) .  I compiled my own libwiiuse.so and it is already installed.  I’ll try the supplied libWiiuseJ.so, which “should” work since its just a simple java / jni interface library. 

Wheeee ! - .so / .dll hell !  /usr/lib/libWiiuseJ.so: libbluetooth.so.2: cannot open shared object file: No such file or directory

Juggling binaries now … wheee !

Yay ! substituted supplied so’s made symbolic link to libbluetooth - app started !  (goodtimes)

Here is a data frame from WiiUseJ simple app example

/******** Buttons for Wiimote generic Event /
/ Buttons /
— Buttons just pressed : 8
— Buttons just released : 0
— Buttons held : 0
/ IR Tracking /
— Active : true
— calculated X coordinate : 0
— calculated Y coordinate : 0
— calculated Z coordinate : 0.0
— calculated distance : 0.0
— absolute X coordinate : 774
— absolute Y coordinate : 254
— IR virtual screen x resolution : 560
— IR virtual screen y resolution : 420
— IR X correction offset : 0
— IR Y correction offset : 110
— IR Sensitivity (between 1-5) : 3
— aspect ratio of the screen : 4/3
— IR sensor bar position. : Above
— Seen points
/ Motion sensing ********/
— Motion sensing : true 
— Orientation threshold value ? : 0.5
— Acceleration threshold value ? : 5
— Alpha smoothing threshold value ? : 0.07
— Smoothing ? : true
— Orientation : (roll: 1.9038614, pitch: -14.434792, yaw: 0.0, absolute roll: 0.0, absolute pitch: -12.094757)
— Gravity force : (0.0, -0.21428572,1.1071428)
— Raw acceleration : (131, 134,168)
References:
WEBCAM OPTICAL TRACKING

2

Just a Thought…

Just for you, my man.

http://www.youtube.com/watch?v=7svYzpTvZDs

3

Thanks CTC

Nice to see you again.  Clever way to mount a prism or mirror to get 360 degree coverage.

4

One Wii Step in the Right Direction

IMG_0001.jpg

Got a lil O'le tank for my B-Day ... it actually lasted a couple weeks before being dis-assembled.  The original RF was 24.7 Mhz.  I have a pair of 434 Mhz Sparkfun rx-tx's,  But it would seem to me that the preferred method of control would be using the bluetooth of the wii-controller.  I pretty sure I have a library which will run on the PC and could send data to the wii, I already got the data going the other way no problem.  

Gareth you are a one man band, I see you hacked nearly every single Nintedo product !   I was thinking of hooking an Arduino up to the I2C coming off of the motion-plus modules.

Would that be your recommondation?

So I think the basic plan is :

PC -> MyRobotLab -->  PC BlueTooth Dongle-> BlueTooth -->  WiiMote on Tank -- I2C --> Arduino --> Motor control/sensors etc

Then going the other way:

WiiMote data + Arduino data --> BlueTooth --> PC Dongle -> MyRobotLab (To Assemble/Visualize SLAM data)

 

Alien Screws?  It's like they don't want us to hack it?

IMG_0002.jpg

5

Marginal results

I got the IR filter out, but it does not seem to have range of over a meter?   I tried a toilet paper roll shield but it did not improve substantially.  Suggestions?

6

Got Data

I got data flow’in into the RobotLab, also noticed that there is a sensitivity level. Set to 5 max - and the range is better.  Noticed the library can turn on/off 4 LED lights !   Sweet, just needed 2 bits to control the tank !  Must hack those LEDs now !  At some point I’ll want to put an Arduino on the tank too.  

A Servo platform to move the WiiDAR would be nice too…

 

wiidar_1.png
wiidar_1.png500×498 31.5 KB

 

7

Left is not Right

I have been experimenting with a servo and laser WiiDAR…  Initially I was interested in getting the servo to go full speed (10 ms pause between positions - under 10 ms gives very chaotic results)

I was shocked to learn that Servos go one direction much faster than they do the other direction !  Also, I roughly found the sampling rate of the IR camera in the WiiMote… and became very frustrated with the crappy, non-linear, too complicated for my math skills math which would be needed for all the different discrepancies.  I will change my tack, and go with the (block and wait to stabilize) method I used for the Genetic Programming Frog Leg.

This is the slow direction - 28 samples were captured

right_99.jpg

This was the fast direction - only 12 samples out of 40 possibilities

left_98.jpg

more to come .....

 

8

Lookin better…

Turns out I was an idiot and resetting the servo in on direction yet stepping at 10ms intervals in the other… no wonder it was more speedy in on direction… ooops

I slowed it down anyway to 30ms intervals and it seem pretty stable.

Now its time to do some math…

w_6_png.jpg

9

Wall @ 36"

2011.01.08

First WiiDAR pictures - wall at 36"

The good - got most of the math correct I believe, the refresh rate is about 1 second (ya - thats good)
The bad - range is not all that great - many material absorb enough of the laser light to make it invisible to the wii - there is a problem with the left right scan, maybe an ObiWan (off by one) error? right scan is not accurate.  I believe the matching of the IR / Laser would provide a significant boost in range & accuracy.
The ugly - the ends of the scan are goofed up do to mechanical complexities / lag time / change of direction of the servo (time to get a motor as Chris pointed out and encoders)  Although, one benefit of this is the ease  1 wii 1 servo 1 uC to move the servo only,  specifically no circuitry to read encoders or move a motor.  The benefits are too great though not to use a motor.  It would probably be smoother, more accurate, less irritating to listen to.  In addition, I had to right a lot of “specialty” code for the servo.

10

code check

public double computeDepth (IRData ir) { //int pixels = 1023 - 473; int pixels = 1023 - ir.event.getAx(); // range point double A; // laser static angle double B; // camera angle double C; // point’s angle double a; // this is what I want it “should be” distance from the wii camera //double b; // distance from the laser double c = 4; // distance from camera to laser A = Math.toRadians(64.5); if (pixels > width/2) { // obtuse triangle B = Math.toRadians(90 + ((pixels - width/2)/pixelsPerDegree)); // get camera angle TODO - what to do past 90? } else { // acute triangle B = Math.toRadians(90 - ((width/2 - pixels)/pixelsPerDegree)); // get camera angle TODO - what to do past 90? } C = Math.toRadians(180) - (B + A); // Law of Sines a = (c * Math.sin(A))/Math.sin©; return a; }

11

\n

unfortunately those code tags don’t tke care of your newlines/carriage returns (or lack thereof)

12

I think this would fix that

and add syntax highlighting in some cases

http://drupal.org/project/codefilter