Continued annotating the code from last year. Progress so far is attached.
We certainly understood the code more, but we are nowhere close to understanding the whole code. We understand the console.log and what require means now. We also have a clue on what's going on in the pCommand and pCommandTimer section.
//ardroneAutonomousControl.js
//image = 640x360
//Blob detection
//Horizontal tracking
//Vertical tracking
//Marks Radius
/* AGENDA
√ Find blobs
√ Record edge links
√ Test bottom camera
√ Test if edge link detection is done accurately by marking them NOTE: I'm wondering if links should store an edge? var, if edge finding is asynchronous at all.
√ Fix glitches with blob detecting
√ (skipping blobs)
√ (green on bottom and right borders of the image)
√ Record radii from center to edge links
√ Record max-min radial difference
√ Find blob with largest difference (not average difference)
√ Get blob line
√ Find blob line direction
√ Mark path
√ Test + fix path marking
√ Use Ø(droneDirection-blobDirection) to control Yaw angle
√ Use bottom camera
• Incorporate second color for junctions, with original functions
√ Try getting navdata for its orientation to control more accurately it's drift
√ Figure out how to read navdata (it's not a straight string...)
√ Use edge pixels when finding junctions and clean up analyzeBlobs()
√ Incorporate navdata to help hovering
√ Fix the "max call stack size exceeded" error: don't use recursion for finding blobs anymore.
√ Fix new errors with findBlobsNoRecursion(): out-of-bounds[√], infinitely-large-blob[√] = problem: pixels that are already links are added as news.
√ Look up Hough functions that could possibly find lines and replace findBlobsNoRecursion()
• Fix drone movement:
try not updating line data if no new line is found [x],
don't do any command other than HOVER 2x in a row [x],
allow drone to do same command twice w/ timer [?],
have path shoulders which help if the drone is lost [ ]
try sending initial navdata-enabling command to see if altitude and velocity data becomes available [ ]
> the command is:
> client.config('general:navdata_demo', 'FALSE');
*/
/* COLOR KEY:
WHITE: line marker
GRAY: junction marker
RED: radius
BLUE: center, best path estimation
YELLOW: path direction head
GREEN: edge
*/
var ardrone = require('ar-drone') //the varaible ardrone requires the information from ar-drone
var jimp = require('./jimp-master/index.js')
var math = require('./mathjs-master/index.js') //Comprehensive math library (used for square root, exponents, absolute value, vector math, etc.)
//Navdata
var orientation = [0.000,0.000,0.000] //direction facing
var origin = [0,0,0] //start location
var client = ardrone.createClient()
var pngImage //640*360
var markerX = -1
var markerY = -1
var markerR = -1
var pathX = -1
var pathY = -1
var pathA = -1
var erosionFactor = 2
var count = 0
var skipSize = 10
var command = [0,0] //0,1,2,3,4
var pCommand = [0,0] //0,1,2,3,4 = HOVER,UP,RIGHT,DOWN,LEFT
var pCommandTimer = [0,0]; //counts how long the drone has been trying the same command
var timeOffCourse = 0;
var color1 = [240,100,100]
var color2 = [240,172,110]
var blobsFound = new BlobLibrary()
client.config("video:video_channel", 1)
var pngStream = client.getPngStream()
pngStream
.on("error", console.log)
.on("data", function(incoming) {
processImage(incoming)
})
client.on("navdata", function(navdata) { //regulates the drone's movement?
getMotionData(navdata)
if (pCommand[0] == command[0]) { //if pCommand[0] is equal to command [0], add
pCommandTimer[0]++ //adds one to the commandTimer
}
else {
pCommandTimer[0] = 0 //stays the name
}
if (pCommandTimer[0] > 50) {
pCommand[0] = 0
}
else {
pCommand[0] = 0
}
if (pCommand[1] == command[1]) {
pCommandTimer[1]++
}
else {
pCommandTimer[1] = 0
}
if (pCommandTimer[1] > 45) {
pCommand[1] = 0
}
else {
pCommand[1] = 0
}
controlFlight()
count++
})
if (count < 30) {
client.takeoff()
}
//.................................................................... DECLARATION
function getMotionData(navdata) { //I wanted to stabilize the drone by countering it's lean
if (count > 10) {
if (count < 30) { //origin = beginning
origin[0] = navdata.demo.rotation.roll
origin[1] = navdata.demo.rotation.pitch
origin[2] = navdata.demo.rotation.yaw
}
else { //orientation = facing
orientation[0] = navdata.demo.rotation.roll
orientation[1] = navdata.demo.rotation.pitch
orientation[2] = navdata.demo.rotation.yaw
}
}
}
function controlFlight() { //Control drone based on given path (X,Y,A)
if (count < 500 && count > 50) {
if (pathA > -1 && pathX > -1 && pathY > -1) {
var distance = math.sqrt(math.pow(pathX-(640*0.5),2) + math.pow(pathY-(320*0.5),2))
var angleV = math.pi * 1.5
angleV = pathA - angleV
if (distance > 320/3) { //CENTER OVER THE PATH OR MOVE FORWARD
timeOffCourse++;
var xMore = false;
var xV = pathX - (640*0.5)
var yV = pathY - (320*0.5)
if (math.abs(xV) > math.abs(yV)) {
xMore = true;
}
xV /= math.abs(xV)
yV /= math.abs(yV)
if ((timeOffCourse*0.001) < 0.04) {
xV *= 0.05 - (timeOffCourse*0.0005)
yV *= 0.05 - (timeOffCourse*0.0005)
}
else {
xV *= 0.005; //0.01
yV *= 0.005;
}
if (xV > 0.0) {
command[0] = 2
}
else if (xV < 0.0) {
command[0] = 4
}
if (yV > 0.0) {
command[1] = 3
}
else if (yV < 0.0) {
command[1] = 1
}
client.stop()
if ((pCommand[1] == 0 || pCommand[1] != command[1]) && !xMore) {
if (command[1] == 1) {
client.front(math.abs(yV))
console.log("FRONT")
}
else if (command[1] == 3) {
client.back(math.abs(yV))
console.log("BACK")
}
}
if ((pCommand[0] == 0 || pCommand[0] != command[0]) && xMore) {
if (command[0] == 2) {
client.right(math.abs(xV))
console.log("RIGHT")
}
else if (command[0] == 4) {
client.left(math.abs(xV*1.5))
console.log("LEFT") //if certain conditions are met, then it will display which direction to the move the drone?
}
}
}
else {
timeOffCourse = 0;
if (distance < 320/3 && math.abs(angleV) > 0/*(math.pi*0.1)*/) { //ROTATE
client.stop()
if (math.abs(angleV) < (math.pi*0.5)) {
if (angleV > 0) {
client.clockwise(0.1)
console.log("CLOCK")
}
else if (angleV < 0) {
client.counterClockwise(0.1)
console.log("COUNTER")
}
}
else {
console.log("PATH IS PERPENDICULAR")
}
}
if (distance < 320/3) { //HOVER
// if (orientation[0] < origin[0]-4) {
// client.right(0.08)
// }
// else if (orientation[0] > origin[0]+4) {
// client.left(0.08)
// }
// if (orientation[1] < origin[1]-4) {
// client.back(0.08)
// }
// else if (orientation[1] >origin[1]+4) {
// client.front(0.08)
// }
client.stop()
client.front(0.02);
command = [0,0]
console.log("PATH FOUND :)") //found path
}
}
}
else { //HOVER
// if (orientation[0] < origin[0]-4) {
// client.right(0.08)
// }
// else if (orientation[0] > origin[0]+4) {
// client.left(0.08)
// }
// if (orientation[1] < origin[1]-4) {
// client.back(0.08)
// }
// else if (orientation[1] > origin[1]+4) {
// client.front(0.08)
// }
command = [0,0]
console.log("LOST :(") //can't locate path
}
}
else {
if ((count > 500 || count == 500) && count < 510) { //if count meets criteria, drone lands
client.stop()
client.land()
}
}
}
function processImage(input) { //Find path and junction in image
pngImage = input
jimp.read(pngImage, function(err, image) {
if (err) throw err
image = thresholdImage(image)
findBlobsNoRecursion(image)
analyzeBlobs()
var line = findLines()
// var marker = findJunctions()
//
// if (marker[0] > -1 && marker[1] > -1) {
// image.setPixelColor(jimp.rgbaToInt(255,0,0,255),marker[0],marker[1])
// for (var i=0; i<marker[2]; i++) {
// if (marker[0] + i + 1 < image.bitmap.width) {
// image.setPixelColor(jimp.rgbaToInt(255,0,0,255),marker[0]+i+1,marker[1])
// }
// }
// }
// else {
// //console.log("NO JUNCTIONS")
// }
if (line[0] > -1 && line[1] > -1 && line[2] > -1) {
var vectorX = math.cos(line[2]) * 1
var vectorY = math.sin(line[2]) * 1
for (var i=1; i<20; i++) {
image.setPixelColor(jimp.rgbaToInt(0,100,255,255),line[0] + math.round(vectorX*i),line[1] + math.round(vectorY*i))
}
image.setPixelColor(jimp.rgbaToInt(255,255,0,255),line[0] + math.round(vectorX*20),line[1] + math.round(vectorY*20))
pathX = line[0]
pathY = line[1]
pathA = line[2]
}
else {
//console.log("NO LINES")
}
markBlobs(image)
//image.write("./droneControlOutput/img_" + count + ".png")
// markerX = marker[0]
// markerY = marker[1]
// markerR = marker[2]
})
}
function thresholdImage(image) { //Color thresholding (looking at image to figure things out, color-wise)
for (var y = 0; y < image.bitmap.height - skipSize; y += skipSize) {
for (var x = 0; x < image.bitmap.width - skipSize; x += skipSize) {
var color = jimp.intToRGBA(image.getPixelColor(x,y))
if (color.r / color.b > (color1[0]/color1[2]) - 1.5 && color.r / color.b < (color1[0]/color1[2]) + 2.5 && color.r / color.g > (color1[0]/color1[1]) - 1 && color.r / color.g < (color1[0]/color1[1]) + 2.5) { //~ORANGE
image.setPixelColor(jimp.rgbaToInt(255,255,255,255),x,y)
}
/*else if (color.r / color.b > (color2[0]/color2[2]) - 0.5 && color.r / color.b < (color2[0]/color2[2]) + 0.5 && color.r / color.g > (color2[0]/color2[1]) - 0.5 && color.r / color.g < (color2[0]/color2[1]) + 0.5) { //GREEN
image.setPixelColor(jimp.rgbaToInt(100,100,100,255),x,y)
}*/
else {
image.setPixelColor(jimp.rgbaToInt(0,0,0,255),x,y)
}
}
}
return image
}
function findBlobsNoRecursion(image) { //Find groups of pixels of the same color (grouping colors)
blobsFound.blobs = [] //clear blobs from previous image
var pixNums = [0,0] //just to keep track of how many pixels were kept vs. how many were not after thresholding
for (var startY = 0; startY < image.bitmap.height - skipSize; startY += skipSize) { //Loop through all pixels (accounting for skipSize) in the image
for (var startX = 0; startX < image.bitmap.width - skipSize; startX += skipSize) {
var color = jimp.intToRGBA(image.getPixelColor(startX,startY)) //Get color of current pixel (startX,startY)
var inBlob = false
if (color.b > 0) { //**COMMENT NOT FOR MR LIN** type1 = 255, type2 = 100
pixNums[0]++
for (var i=0; i<blobsFound.blobs.length; i++) { //Loop through all blobs found so far to check if current pixel has already been used
for (var j=0; j<blobsFound.blobs[i].links.length && inBlob == false; j++) {
if (blobsFound.blobs[i].links[j].x == startX && blobsFound.blobs[i].links[j].y == startY) {
inBlob = true
}
}
}
}
else {
pixNums[1]++
}
if (inBlob == false && color.b > 0) { //If pixel is within threshold and not already used, then create a new blob
var edges = [] //A selection of links that will be used to find blob radii outside of findBlobsNoRecursion()
var links = [] //Points that will make up the new blob
var news = [] //Points that haven't been checked yet for new neighboring white pixels
news.push(new Link(startX,startY)) //Add first pixel to news
var iteration=0 //Just for me to see how long it takes for the program to finish the blob
while (news.length > 0) { //While there are still pixels whose neighbors are not checked...
var len = news.length //Number of pixels which, as of now, aren't checked
for (var i = len-1; i > -1; i--) { //Loop through current news[] pixels from last to first (won't include pixels added to the array later in the process)
var x = news[i].x //store location of new pixel to be checked
var y = news[i].y
if (y-skipSize > 0 && y+skipSize < image.bitmap.height && x-skipSize > 0 && x+skipSize < image.bitmap.width) { //make sure new pixel is not at the edge of the image
color = jimp.intToRGBA(image.getPixelColor(x,y-skipSize)) //START: check neighbor above
if (color.b == 255) { //if neighbor is white
var used = false
for (var j=0; j<news.length && used == false; j++) { //loop through new pixels
if (news[j].x == x && news[j].y == y-skipSize) { //check if neighbor is already added
used = true
}
}
if (used == false) {
for (var j=0; j<links.length && used == false; j++) { //loop through saved pixels (already in blob)
if (links[j].x == x && links[j].y == y-skipSize) { //check if neighbor is already used
used = true
}
}
if (used == false) {
news.push(new Link(x,y-skipSize)) //add neighbor to news[]
}
}
} //END: check neighbor above
color = jimp.intToRGBA(image.getPixelColor(x,y+skipSize)) //START: check neighbor below
if (color.b == 255) {
var used = false
for (var j=0; j<news.length && used == false; j++) {
if (news[j].x == x && news[j].y == y+skipSize) {
used = true
}
if (used) {
break
}
}
if (used == false) {
for (var j=0; j<links.length && used == false; j++) {
if (links[j].x == x && links[j].y == y+skipSize) {
used = true
}
}
if (used == false) {
news.push(new Link(x,y+skipSize))
}
}
} //END: check neighbor below
color = jimp.intToRGBA(image.getPixelColor(x-skipSize,y)) //START: check neighbor left
if (color.b == 255) {
var used = false
for (var j=0; j<news.length && used == false; j++) {
if (news[j].x == x-skipSize && news[j].y == y) {
used = true
}
}
if (used == false) {
for (var j=0; j<links.length && used == false; j++) {
if (links[j].x == x-skipSize && links[j].y == y) {
used = true
}
}
if (used == false) {
news.push(new Link(x-skipSize,y))
}
}
} //END: check neighbor left
color = jimp.intToRGBA(image.getPixelColor(x+skipSize,y)) //START: check neighbor right
if (color.b == 255) {
var used = false
for (var j=0; j<news.length && used == false; j++) {
if (news[j].x == x+skipSize && news[j].y == y) {
used = true
}
}
if (used == false) {
for (var j=0; j<links.length && used == false; j++) {
if (links[j].x == x+skipSize && links[j].y == y) {
used = true
}
}
if (used == false) {
news.push(new Link(x+skipSize,y))
}
}
} //END: check neighbor right
}
if (isEdge(image,x,y,1)) { //check if new pixel is an edge
edges.push(new Link(x,y)) //add new pixel to edges[] (for calculating blob's radii later)
}
links.push(news[i]) //add this pixel to the new blob
news.splice(i,1) //remove this pixel from news[], as it's now checked
}
iteration++
}
if (links.length > 5) { //only add blob if it's size is somewhat significant
//console.log("...BLOB ADDED @ " + startX + "," + startY) //print blob's initial point
blobsFound.addBlob(1) //add an empty blob (constructor is not currently important)
blobsFound.blobs[blobsFound.blobs.length-1].links = links //fill blob's links[] array
blobsFound.blobs[blobsFound.blobs.length-1].edges = edges //fill blob's edges[] array
}
else {
//console.log("BLOB TOO SMALL")
}
}
}
}
//console.log("+: " + pixNums[0] + ", -: " + pixNums[1]) //not important
}
function isEdge(image, x, y, type) { //Edges used for finding the radii of a blob
var neighbors = 0
var color
if (x+skipSize < image.bitmap.width && y-skipSize > 0) {
color = jimp.intToRGBA(image.getPixelColor(x+skipSize,y-skipSize))
if (color.b == 255) {
neighbors++
}
}
if (x+skipSize < image.bitmap.width) {
color = jimp.intToRGBA(image.getPixelColor(x+skipSize,y))
if (color.b == 255) {
neighbors++
}
}
if (x+skipSize < image.bitmap.width && y+skipSize < image.bitmap.height) {
color = jimp.intToRGBA(image.getPixelColor(x+skipSize,y+skipSize))
if (color.b == 255) {
neighbors++
}
}
if (y+skipSize < image.bitmap.height) {
color = jimp.intToRGBA(image.getPixelColor(x,y+skipSize))
if (color.b == 255) {
neighbors++
}
}
if (x-skipSize > 0 && y+skipSize < image.bitmap.height) {
color = jimp.intToRGBA(image.getPixelColor(x-skipSize,y+skipSize))
if (color.b == 255) {
neighbors++
}
}
if (x-skipSize > 0) {
color = jimp.intToRGBA(image.getPixelColor(x-skipSize,y))
if (color.b == 255) {
neighbors++
}
}
if (x-skipSize >0 && y-skipSize > 0) {
color = jimp.intToRGBA(image.getPixelColor(x-skipSize,y-skipSize))
if (color.b == 255) {
neighbors++
}
}
if (y-skipSize > 0) {
color = jimp.intToRGBA(image.getPixelColor(x,y-skipSize))
if (color.b == 255) {
neighbors++
}
}
if (neighbors > 1 && neighbors < 7) {
return true
}
else {
return false
}
}
function markBlobs(image) { //Show where the program found blobs
for (var i=0; i<blobsFound.blobs.length; i++) {
if (blobsFound.blobs[i].links.length > 5) {
var location = [blobsFound.blobs[i].aspects[0],blobsFound.blobs[i].aspects[1]]
image.setPixelColor(jimp.rgbaToInt(0,100,255,255),math.round(location[0]),math.round(location[1]))
for (var j=0; j<blobsFound.blobs[i].edges.length; j++) {
location = [blobsFound.blobs[i].edges[j].x,blobsFound.blobs[i].edges[j].y]
image.setPixelColor(jimp.rgbaToInt(0,255,0,255),location[0],location[1])
}
}
}
}
function analyzeBlobs() { //Calculate data of a blob
for (var i=0; i<blobsFound.blobs.length; i++) {
blobsFound.blobs[i].calculateCenterRadii()
if (blobsFound.blobs[i].aspects[7] == 1) {
blobsFound.blobs[i].calculateLinearityDirection()
}
else if (blobsFound.blobs[i].aspects[7] == 2) {
blobsFound.blobs[i].calculateCircularity()
}
}
}
function findLines() { //Use blob data to find most likely path
var Lnum = 0;
var bestLine = [2]
bestLine[0] = 0
bestLine[1] = 0
for (var i=0; i<blobsFound.blobs.length; i++) {
if (blobsFound.blobs[i].aspects[7] == 1 && blobsFound.blobs[i].links.length > 10) {
if (blobsFound.blobs[i].aspects[5] > bestLine[0]) {
bestLine[0] = blobsFound.blobs[i].aspects[5]
bestLine[1] = i
}
Lnum++
}
}
if (blobsFound.blobs.length > 0 && Lnum > 0) {
var lineHeading = blobsFound.blobs[bestLine[1]].aspects[6]
var angleDifference = math.abs((math.pi*1.5) - lineHeading)
if (angleDifference > math.pi) {
angleDifference = (2*math.pi) - angleDifference
}
if (angleDifference > 0.5*math.pi) {
lineHeading += math.pi
}
if (lineHeading > 2*math.pi) {
lineHeading -= 2*math.pi
}
var lineData = [blobsFound.blobs[bestLine[1]].aspects[0],blobsFound.blobs[bestLine[1]].aspects[1],lineHeading]
}
else {
var lineData = [-1,-1,-1]
}
return lineData
}
function findJunctions() { //Use blob data to find most likely junction
var Jnum = 0
var bestCircularity = [2] //circularity, blob#
bestCircularity[0] = 20
bestCircularity[1] = 0
var bestDensity = [2] //density, blob#
bestDensity[0] = 0
bestDensity[1] = 0
var bestBlob = 0
for (var i=0; i<blobsFound.blobs.length; i++) {
if (blobsFound.blobs[i].aspects[7] == 2 && blobsFound.blobs[i].links.length > 20) {
Jnum++
var circularity = blobsFound.blobs[i].aspects[3]
if (circularity < bestCircularity[0]) {
bestCircularity[0] = circularity
bestCircularity[1] = i
bestBlob = i
}
var density = blobsFound.blobs[i].aspects[4] //Not used right now...
}
}
if (blobsFound.blobs.length > 0 && Jnum > 0) {
var junctionData = [blobsFound.blobs[bestBlob].aspects[0],blobsFound.blobs[bestBlob].aspects[1],blobsFound.blobs[bestBlob].aspects[2]]
}
else {
var junctionData =[-1,-1,-1]
}
return junctionData
}
function BlobLibrary() {
this.blobs = []
}
BlobLibrary.prototype.addBlob = function(color) {
this.blobs = this.blobs.concat(new Blob(color))
}
function Blob(color) {
this.links = []
this.edges = []
this.radii = []
this.aspects = [8]
this.aspects[0] = 320 //X
this.aspects[1] = 200 //Y
this.aspects[2] = 50 //R adius
this.aspects[3] = 3 //C ircularity
this.aspects[4] = 5 //D ensity
this.aspects[5] = 0 //L inearity
this.aspects[6] = 0 //A ngle
this.aspects[7] = color //C olor (1=line,2=junction)
}
Blob.prototype.addLink = function(x, y) {
this.links = this.links.concat(new Link(x, y))
}
Blob.prototype.addEdge = function(x, y) {
this.edges = this.edges.concat(new Link(x, y))
}
Blob.prototype.calculateCenterRadii = function() {
var X = 0
var Y = 0
var edgeRadii = [this.edges.length]
for (var i=0; i<this.links.length; i++) {
X += this.links[i].x
Y += this.links[i].y
}
X /= this.links.length
Y /= this.links.length
this.aspects[0] = X
this.aspects[1] = Y
for (var i=0; i<this.edges.length; i++) {
var edgeRadius = math.sqrt(math.pow(this.edges[i].x - this.aspects[0],2) + math.pow(this.edges[i].y - this.aspects[1],2))
edgeRadii[i] = edgeRadius
}
this.radii = edgeRadii
if (this.radii.length > 0) {
var avgRadius = 0
for (var i=0; i<this.radii.length; i++) {
avgRadius += this.radii[i]
}
avgRadius /= this.radii.length
this.aspects[2] = avgRadius
}
}
Blob.prototype.calculateCircularity = function() {
if (this.radii.length > 0) {
var avgDifference = 0
for (var i=0; i<this.radii.length; i++) {
avgDifference += (this.radii[i] - this.aspects[2])
}
avgDifference /= this.radii.length
this.aspects[3] = avgDifference
}
this.aspects[4] = this.links.length / this.aspects[2]
}
Blob.prototype.calculateLinearityDirection = function() {
var shortest = 700
var longest = 0
var arrow = [1,1]
for (var i=0; i<this.radii.length; i++) {
var edgeRadius = this.radii[i]
if (edgeRadius < shortest) {
shortest = edgeRadius
}
if (edgeRadius > longest) {
longest = edgeRadius
arrow[0] = this.edges[i].x - this.aspects[0]
arrow[1] = this.edges[i].y - this.aspects[1]
}
}
var linearity = longest - shortest
this.aspects[5] = linearity
var angle = math.atan2(math.abs(arrow[1]), math.abs(arrow[0]))
if (arrow[0] < 0 && arrow[1] > 0) {
angle = math.pi - angle
}
else if (arrow[0] < 0 && arrow[1] < 0) {
angle = math.pi + angle
}
else if (arrow[0] > 0 && arrow[1] < 0) {
angle = (2*math.pi) - angle
}
this.aspects[6] = angle
}
function Link(x, y) {
this.x = x
this.y = y
}
No comments:
Post a Comment