-
Stereo Coplanar Area Matching Principal Component Analysis Parallel (SCAMPCAP)
08/14/2019 at 21:36 • 0 commentsStereo Coplanar Area Matching Principal Component Analysis Parallel (SCAMPCAP)
The following reflects improvements to the previously published work on 3D Depth reconstruction from stereo camera images for machine vision. The core of the new algorithm relies on Principal Component Analysis (PCA) to match key epipolar coplanar regions in the 2 stereo images. Selection of these corresponding coplanar regions seems to mimic the manner in which biological systems extract key features for visual cortex processing. Several improvements to the previously published algorithm were made to increase accuracy. Efficiency gains were realized through more accurate region matching, constraining search areas, and finally, paralleling of the process. The Java threading model with cyclic barrier synchronization is the mechanism by which the parallel processing is achieved. Performance remains suitable for realtime applications and is immune to calibration errors and color balance. the code is operational and in place on a number of robotic test platforms in both indoor and outdoor environments.
Improved algorithm for stereo matching:
(The terms region, node, and octree node and octree cell are synonymous)
The steps in the pipeline are:
1.) Edge detect both images using Canny edge detection. A single thread performs these operations.
2.) Generate 2 octrees of edge detected images at the minimal octree node level, now 7. The number of threads in the octree build process is proportional to the image scan height. A pool of threads that is 1/10 the scan height of the images is used to process each scan line of the images in parallel.
3.) Use Principal Component Analysis (PCA) on the 2 images to find the minimal coplanar regions in both octrees and generate the eigenvectors and eigenvalues of the principal axis of the minimal coplanar regions.
// A typical barrier synchronized parallel execution block // this one manages the building of octrees from the stereo image data for(int syStart = 0; syStart < execLimit; syStart++) { SynchronizedFixedThreadPoolManager.getInstance(numThreads, execLimit).spin(new Runnable() { @Override public void run() { imagesToOctrees(dataL, dataR, imageLx, imageRx, yStart.getAndIncrement(), camWidth, camHeight, nodel, noder); } }); SynchronizedFixedThreadPoolManager.getInstance().waitForGroupToFinish(); } // for syStart
4.) Sort the resulting collections of minimal coplanar regions by their centroid Y values.
5.) Process the left image coplanar regions against the right coplanar regions by comparing vector cross products of the two secondary eigenvector axis. Limit processing regions to those whose centroids are within a preset tolerance of the difference in Y value of the 2 centroids. Currently, 25 pixels is used as a tolerance. If more than one candidate is found, select the one with minimal difference in eigenvector cross product. Parallel work is assigned using the sorted centroid Y values to partition the processing into blocks of regions with centroids of common Y value.
6.) Assign a disparity to the minimal octree region based on differences in linear distance of the 2 octree centroids of the points in the coplanar minimal regions from step 4. The regions are small enough that centroid value is accurate enough to give a reasonable disparity value. Work is now assigned to parallel threads based on the size of the collection of minimal regions from the left image, as it is the left image that drives the process of depth assignment. The thread pool is set to a subset of the collection size and the maximum number of threads is set at the number of elements in the collection.
7.) Calculate the mean, variance, and standard deviation of the depth of all the minimal regions, once assigned by step 6.
8.) Reset the left image octree and regenerate the coplanar regions via PCA at a higher octree level, using octree level 5.
9.) Find the minimal regions (that were assigned a depth) enclosed by the newly generated larger coplanar regions.
10.) Find the mean of all the depths in the subsample of minimal regions enclosed in the larger coplanar region.
11.) Remove from consideration any of the minimal regions whose depth is 2 sigma above the mean of all the minimal regions calculated in step 7.
12.) For all points in the larger regions, determine which enclosed minimal region they fall into. If within the smaller region, assign the point the depth we found and assigned to that minimal region earlier in step 6.
13.) For points in the larger region that do not fall into a smaller enclosed region, assign it the mean of the depth of the subsampled regions from step 10.
14.) After processing a larger region, remove the smaller regions it encloses from consideration.
15.) If any minimal regions remain, place them in a 'no-confidence' collection along with unmatched minimal regions from step 5. The collection of 'no-confidence' minimal regions can be used to indicate the possible presence of occlusions that prevent the images from being matched. These occlusions occur when an object appears in one camera but not the other. Conditions where occlusion occurs can include objects closer than the image baseline, or objects positioned in such a way that an object blocks the view of one camera.
16.) Place in 2 separate collections those maximal regions that enclose zero minimal regions, and the maximal regions that were assigned a depth from the mean of the enclosed minimal regions in step 10. A larger region may enclose zero minimal regions when an overlapping region has processed and 'used up' all the minimal regions it encloses.
17.) Process the collection of maximal regions that enclose zero minimal regions in parallel threads based on the size of the collection. For each 'zero region', determine the 'depth regions' it encloses, or the 'depth regions' that enclose it, and create a collection of that subsample for each 'zero region' in each thread.
18.) For every point in the 'zero region', determine the enclosing/enclosed 'depth region' it falls into. If it falls into one of these regions, assign the point the depth associated with the corresponding 'depth region'. If the point falls into none of the 'depth regions', assign the point the mean of all the 'depth regions' enclosed by the 'zero region' of the point, much as we did at a smaller level in steps 9, 10, 11, 12 and 13.
So the pipeline determines the matching epipolar minimal coplanar regions, uses those to apply that disparity to a larger common coplanar region as depth, then uses the larger coplanar regions to assign larger, but still common, coplanar regions a depth. The algorithm seems to consistently result in a 97% depth assignment rate.
Testing is still underway using 2 different robots in indoor and outdoor conditions but sub-1-second response time using 640 by 480 pixel images from standard webcams at a 205mm baseline and a 4.5mm focal length is achieved. The code implementing SCAMPCAP, documentation, and video of test platform robots in the field using code from my various projects, appear in the repositories at github.com/neocoretechs
Final 3D SCAMPCAP point cloud rendered from depth assigned edge detected image, rendered slightly off axis, with minimal coplanar regions (small tricolor areas), unmatched minimal regions (small blue squares), and maximal octree nodes (large blue squares).
-
ROSCOEs RosJavaLite powering 24v gear motor surplus wheelchair
03/12/2018 at 21:24 • 0 commentsPermobil C400 robot, former surplus mobility chair. ROS Java Lite drives the entire system. This is one of 2 robots using radically different propulsion and hardware but the same exact code dropped in except for 1 config file, which is the point of the exercise. A remote PS3 controller and first person viewer with 5x7 color screen is used for remote control as the robot contains a wireless N access point.
https://www.linkedin.com/feed/update/urn:li:activity:6379061242572738561
-
New Brain: 24v 9 node supercluster in a box
12/20/2014 at 20:13 • 0 comments -
ROSCOES New Brain: Supercluster in a Shoebox
12/11/2014 at 19:47 • 0 commentsI'm building my dream machine! Purpose built supercluster in a box matched to BigSack software. Not often as a developer you get to build a machine to match your software. I am using 8 ODroid C1 1.5 gHz quad proc Cortex A5 SBC with 1 gig memory each and gigabit ethernet. Also got 8 16GB eMMC cards, about 3x performance from SD flash. Each of these 8 workers will service one tablespace of every data store. Each database is composed of 8 memory mapped tablespaces. The master will be an ODroid XU3 octocore Cortex A15 with 2 gig running at 2 gigahertz. The master handles the redo log files and multiplexes requests to the worker nodes and gets blocks back. The transport is UDP fixed block size @ 4096 bytes. Each worker node is on its own UDP port. A cluster lives and dies by its switching fabric so I got 2 6 port 24V 1000TX industrial switches, those were the most expensive part, spent about a grand on the computer boards but the switches were $1300 apiece. Had to get 2 Dc/Dc converters from 24V to 5V @ 8A to power the worker nodes, those were $300 a pop. Still I doubt there is any mainframe that can match it pound for kilo when its running. When it is, I can transplant it right into ROSCOE himself. I have accepted the fact that I need a separate 24V bus just to power computing.
The eMMC is supposed to be 300 MB/sec so thats 2.4 gigabytes per second IO to permanent storage total aggregate.
8 procs at 1.5 GHz is 12 GHz plus 2 is 14 gigahertz aggregate clock.
8 cores master + 4 cores each node times 8 nodes is 40 total cluster processor cores
10 gigabytes total cluster RAM. 8 x 16 GB nodes plus 64GB master is 180GB eMMC total cluster perm storage (without extra flash, eMMC leaves Micro SD flash space open)
8 gigabit ethernet channels running UDP is total 1 gigabyte per second burst cluster throughput with all worker nodes fully active
After assembly, I will run my new Canny/Hough/Inverse DCT transform and build the AMI Algebraic Machine Intelligence model based on the exemplars of the Caltech 101. I can then test the Caltech 101 full dataset on the new model. After refinement its on to Caltech 256, PASCAL VOC 2007 and whatever else. If I am cracking the Caltech 256 at 90% with my shoebox robot brain attention will follow.
-
ROSCOE's Oriented Roundel
08/31/2014 at 19:07 • 0 commentsNo, its not a band, yet. The ARDrone has a built-in machine vision capability that recognizes certain key images, one of which is shown below. The data stream responds with the estimated distance, which I believe is derived from the ultrasonic sensor, and the orientation in degrees rotation with the below image being at 90 reported degrees. So ROSCOE has a 'toy' that he get agitated and tries to find if he cant see it within 500 CM, if he can he will sit 500CM away and stare at it. In a way its an extremely sophisticated 'off' switch as we move into more motion studies. The movement algorithm is extremely simple; move forward by issuing 50mm forward motion Twist messages on the ROS 'cmd_vel' topic every 500ms, which is a standard ROS communication pattern. Four or five inches per second indoors for this thing is enough, it will push the furniture out of the way, and not just the end tables either. The ARDrone published sensor data and external ultrasonics are partially fused to avoid shocks/off axis and we include the publication of 'ardrone/vision' Pose2D range/orientation message if a roundel is detected by the ARDrone. The 'cmd_vel' movement publisher subscribes to the 'ardrone/vision' topic and if we see the roundel within 500 cm don't issue movement publication on 'cmd_vel' topic for that 500ms interval. If we are moving and an obstacle or shock is encountered, rotate 30 degrees off axis, or if the ultrasonic readings are below 200mm range bottom sensor or 300mm top sensor, back up 50mm, then continue to process Twist messages from the 'cmd_vel' ROS topic. The baseline behavior here will allow us to integrate the functor-based natural transformation cognitive functions now that there is a partial data fusion pipeline of video/accelerometer/gyro/ultrasonic/machine vision/magnetometer.
-
New XmlRpcServer on GitHub
08/30/2014 at 03:14 • 0 commentsROSJava takes a special version of XmlRpcServer with bugfixes. I have added to that by replacing ThreadPool with a ThreadPoolManager singleton that uses java.util.concurrent Executors as intended by modern standards. Thread management is much better and I can see a much faster spinup of the Master and Slave servers. There also seem to be less NPE's but I also refactored ROSJava to do better checks on collection members so nulls are accounted for in 'equals' and 'hashCode' methods. Anyway, onward and upward. New link is to the left.
-
Fun with Obstacles
08/29/2014 at 01:39 • 0 commentsThis is the most powerful robot I have encountered. I lost the lower ultrasonic so it kept getting 0 readings making it think something was right on top of it so of course it spins in circles trying to avoid the 0 distance object, duh. I tried to grab it to shut down the controller but even at 1/10 power it cranked me. It will obviously have to learn that 1 360 deg. rotation is sufficient to decide a stop. Due to the kinematics it seems like enough power to move the machine is enough to render it too much for muscular override due to the weight of the thing.
-
Robusst
08/19/2014 at 18:41 • 0 commentsBus active for about 36 hours with full nav, video, ultrasonics, etc publishing and subscribing. The unit is positioned such that when someone passes by within range of 300mm it will announce "Excuse me, you are nn millimeters too close to my feet" and/or "Excuse me, you are nn millimeters too close to my head" so its easy to confirm the bus is still operational just by walking by, or if the cat is becoming petrified by walking by the lower ultrasonic and causing it to speak only of its 'feet'. The subscriber is listening to 'range' topics from nodes ardrone and robocore (Parrot and floor level ultrasonics) and doing a simple check which results in the VoxHumana speech module activating or not. Occasionally, an error from the RS232 comms between lower ultrasonic->arduino->core process->ROS allows a zero to creep through so you get "Excuse me, but you are zero millimeters too close to my feet". Passive debugging tools are neato! I did lose the Edamax wireless USB dongle that talks to the ARDrone boards from RPi 1, perhaps from heat. It gradually degraded such that errors from the nav thread were popping up in the form of broken DatagramSockets so I added code to reconnect and tickle the port under those conditions but after replacing the dongle, few broken sockets. Also had to work out all the static persistent routes for bootup. Serendipitous security was achieved when I moved the master registration server to the second RPi. RPi 1 is not set up to source route packets to RPi 2 so its not possible to connect to the master from outside of RPi 1. Nobody can hijack your 'bot by connecting wirelessly and spinning up a ROS node. About the most that could be done is to issue disconnects from the wireless network or cut off video or sensor data. So we are finally good to go with a stable, open platform and can move on to the really interesting part of this project.
-
100% Pure Functionality
08/17/2014 at 22:32 • 0 commentsReplaced xuggler dependencies preventing full implementation on Raspian. Modified h264j to be optimized with ROSARDrone. End-to-end testing on RPi successful. Moved master to roscoe2, second RPi, to distribute load. Performance is acceptable at this point. Added VoxHumana speech module to GitHub.
-
Robot Up
08/15/2014 at 23:19 • 0 commentsAfter ensuring that no more localhost bindings were taking place by ensuring the ROS_IP / ROS_HOSTNAME were set the same problem with Connection Refused occurred. Like many problems this one was related to a combination of my own stupidity and a bugs in the system. If ROS encounters a new node registration with the same name as an existing node it will try to replace the slave server, however, it assumes an immediate slave server restart and tries to connect to that as-yet-unstarted server. That is the bug. The stupidity was my accidentally using the same node name as I did for the publisher, causing the situation to begin with. Eventually, after numerous retries and cringe-inducing stack dumps a connection is made to the new server and traffic flows. My initial test case is having ROSCOE listen to range and status and speaking the status message and checking range for values < 100 cm and report that by saying "Excuse me, but you are <nnn> centimeters too close", all of which it now does via the ROS bus on Raspberry Pi!