Skip to content

Commit

Permalink
Added code to better infer LED location (untested).
Browse files Browse the repository at this point in the history 
  - Note: Still need to modify PnP class to do a few iterations even when
    inferring LED location.
  • Loading branch information
@mdarling39
mdarling39 committed Dec 13, 2013
1 parent b356f51 commit 6db7dae
Show file tree
Hide file tree
Showing 27 changed files with 308 additions and 92 deletions.
2 changes: 1 addition & 1 deletion CompleteVision_MAIN/CompleteVision_MAIN.cbp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
<Unit filename="../Config.hpp" />
<Unit filename="../FPSCounter.cpp" />
<Unit filename="../FPSCounter.hpp" />
<Unit filename="../FeatureDetector_Test.cpp" />
<Unit filename="../FlightDataRecording.hpp" />
<Unit filename="../Global.hpp" />
<Unit filename="../KalmanFilter.cpp" />
Expand All @@ -63,7 +64,6 @@
<Unit filename="../Threshold.hpp" />
<Unit filename="../customblobdetector.cpp" />
<Unit filename="../customblobdetector.hpp" />
<Unit filename="../main_threaded.cpp" />
<Unit filename="../morph.hpp" />
<Unit filename="../neon_instructions.h" />
<Unit filename="../v4l2Cap.cpp" />
Expand Down
14 changes: 7 additions & 7 deletions CompleteVision_MAIN/CompleteVision_MAIN.depend
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,10 +61,10 @@
<sys/types.h>
<sys/stat.h>

1385352724 /home/mdarling/Desktop/CompleteVision_MAIN/Global.hpp
1386278073 /home/mdarling/Desktop/CompleteVision_MAIN/Global.hpp
<iostream>

1385360175 /home/mdarling/Desktop/CompleteVision_MAIN/Config.hpp
1386375985 /home/mdarling/Desktop/CompleteVision_MAIN/Config.hpp
"v4l2_c.h"
"LED_Detector.hpp"
<unistd.h>
Expand Down Expand Up @@ -152,7 +152,7 @@
<vector>
<string>

1385359483 source:/home/mdarling/Desktop/CompleteVision_MAIN/PnPObj.cpp
1386715088 source:/home/mdarling/Desktop/CompleteVision_MAIN/PnPObj.cpp
"PnPObj.hpp"

1381786430 source:/home/mdarling/Desktop/CompleteVision_MAIN/serialCommBeagle.cpp
Expand Down Expand Up @@ -215,7 +215,7 @@
<opencv2/opencv.hpp>
"neon_instructions.h"

1385343495 source:/home/mdarling/Desktop/CompleteVision_MAIN/main_threaded.cpp
1386292946 source:/home/mdarling/Desktop/CompleteVision_MAIN/main_threaded.cpp
"Global.hpp"
"Config.hpp"
"v4l2_c.h"
Expand Down Expand Up @@ -263,17 +263,17 @@
<sys/stat.h>
<ctime>

1385352888 source:/home/mdarling/Desktop/CompleteVision_MAIN/LED_Detector.cpp
1386805576 source:/home/mdarling/Desktop/CompleteVision_MAIN/LED_Detector.cpp
"LED_Detector.hpp"

1385346777 /home/mdarling/Desktop/CompleteVision_MAIN/LED_Detector.hpp
1386818986 /home/mdarling/Desktop/CompleteVision_MAIN/LED_Detector.hpp
"Global.hpp"
<opencv2/opencv.hpp>
<vector>
<algorithm>
<stdio.h>

1385350921 source:/home/mdarling/Desktop/CompleteVision_MAIN/FeatureDetector_Test.cpp
1386738559 source:/home/mdarling/Desktop/CompleteVision_MAIN/FeatureDetector_Test.cpp
"Global.hpp"
"Config.hpp"
"v4l2_c.h"
Expand Down
93 changes: 49 additions & 44 deletions CompleteVision_MAIN/CompleteVision_MAIN.layout
View file
Original file line number Diff line number Diff line change
@@ -1,124 +1,129 @@
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<CodeBlocks_layout_file>
<ActiveTarget name="Debug" />
<File name="../v4l2Cap.cpp" open="0" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../PnPObj.cpp" open="1" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="8062" topLine="0" />
<Cursor1 position="4726" topLine="9" />
</Cursor>
</File>
<File name="../PnPObj.hpp" open="0" top="0" tabpos="7" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../v4l2Cap.h" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="2541" topLine="77" />
<Cursor1 position="573" topLine="15" />
</Cursor>
</File>
<File name="../LED_Detector.hpp" open="0" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../customblobdetector.cpp" open="0" top="0" tabpos="6" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="1422" topLine="45" />
<Cursor1 position="8540" topLine="154" />
</Cursor>
</File>
<File name="../CamObj.hpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../Config.hpp" open="1" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="1336" topLine="46" />
<Cursor1 position="3349" topLine="75" />
</Cursor>
</File>
<File name="../BBBSerial.h" open="1" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../ProfilerTool.h" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="79" topLine="0" />
<Cursor1 position="113" topLine="2" />
</Cursor>
</File>
<File name="../BBBSerial.cpp" open="1" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../Threshold.hpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="528" topLine="0" />
<Cursor1 position="1084" topLine="28" />
</Cursor>
</File>
<File name="../LED_Detector.cpp" open="0" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../OCVCapture.cpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="7545" topLine="176" />
<Cursor1 position="11226" topLine="506" />
</Cursor>
</File>
<File name="../Global.hpp" open="1" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../CamObj.hpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="908" topLine="23" />
<Cursor1 position="1336" topLine="46" />
</Cursor>
</File>
<File name="../FPSCounter.hpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../PnPObj.hpp" open="0" top="0" tabpos="7" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="173" topLine="6" />
<Cursor1 position="2541" topLine="77" />
</Cursor>
</File>
<File name="../v4l2Cap.h" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../neon_instructions.h" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="573" topLine="15" />
<Cursor1 position="2967" topLine="72" />
</Cursor>
</File>
<File name="../neon_instructions.h" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../v4l2Cap.cpp" open="0" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="2967" topLine="72" />
<Cursor1 position="8062" topLine="0" />
</Cursor>
</File>
<File name="../CamObj.cpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="3735" topLine="157" />
</Cursor>
</File>
<File name="../v4l2_c.h" open="0" top="0" tabpos="6" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../FeatureDetector_Test.cpp" open="1" top="0" tabpos="6" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="11781" topLine="409" />
<Cursor1 position="5245" topLine="186" />
</Cursor>
</File>
<File name="../FPSCounter.cpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../FPSCounter.hpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="139" topLine="0" />
<Cursor1 position="173" topLine="6" />
</Cursor>
</File>
<File name="../ProfilerTool.h" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../morph.hpp" open="0" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="113" topLine="2" />
<Cursor1 position="444" topLine="0" />
</Cursor>
</File>
<File name="../Threshold.hpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../customblobdetector.hpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="1084" topLine="28" />
<Cursor1 position="1749" topLine="27" />
</Cursor>
</File>
<File name="../Threshold.cpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../LED_Detector.hpp" open="1" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="675" topLine="9" />
<Cursor1 position="1692" topLine="63" />
</Cursor>
</File>
<File name="../customblobdetector.hpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../v4l2_c.h" open="0" top="0" tabpos="6" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="1749" topLine="27" />
<Cursor1 position="11781" topLine="409" />
</Cursor>
</File>
<File name="../Config.hpp" open="1" top="1" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../LED_Detector.cpp" open="1" top="1" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="414" topLine="21" />
<Cursor1 position="11501" topLine="307" />
</Cursor>
</File>
<File name="../BBBSerial.cpp" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="528" topLine="3" />
</Cursor>
</File>
<File name="../FlightDataRecording.hpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="1246" topLine="30" />
</Cursor>
</File>
<File name="../customblobdetector.cpp" open="0" top="0" tabpos="6" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../Threshold.cpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="8540" topLine="154" />
<Cursor1 position="675" topLine="9" />
</Cursor>
</File>
<File name="../morph.hpp" open="0" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../BBBSerial.h" open="0" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="444" topLine="0" />
<Cursor1 position="79" topLine="0" />
</Cursor>
</File>
<File name="../PnPObj.cpp" open="1" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../Global.hpp" open="1" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="12073" topLine="398" />
<Cursor1 position="810" topLine="13" />
</Cursor>
</File>
<File name="../OCVCapture.cpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="../FPSCounter.cpp" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="11226" topLine="506" />
<Cursor1 position="139" topLine="0" />
</Cursor>
</File>
</CodeBlocks_layout_file>
Binary file modified CompleteVision_MAIN/TestImages/frame_0.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified CompleteVision_MAIN/TestImages/frame_0001.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified CompleteVision_MAIN/TestImages/frame_0002.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file removed CompleteVision_MAIN/TestImages/frame_0003.jpg
View file
Binary file not shown.
Binary file removed CompleteVision_MAIN/TestImages/frame_0004.jpg
View file
Binary file not shown.
Binary file removed CompleteVision_MAIN/TestImages/frame_0005.jpg
View file
Binary file not shown.
Binary file removed CompleteVision_MAIN/TestImages/frame_0006.jpg
View file
Binary file not shown.
Binary file removed CompleteVision_MAIN/TestImages/frame_0007.jpg
View file
Binary file not shown.
Binary file removed CompleteVision_MAIN/TestImages/frame_0008.jpg
View file
Binary file not shown.
Binary file modified CompleteVision_MAIN/bin/Debug/CompleteVision_MAIN
View file
Binary file not shown.
Binary file modified CompleteVision_MAIN/gmon.out
View file
Binary file not shown.
Binary file modified CompleteVision_MAIN/obj/Debug/BBBSerial.o
View file
Binary file not shown.
Binary file modified CompleteVision_MAIN/obj/Debug/FPSCounter.o
View file
Binary file not shown.
Binary file modified CompleteVision_MAIN/obj/Debug/KalmanFilter.o
View file
Binary file not shown.
Binary file modified CompleteVision_MAIN/obj/Debug/PnPObj.o
View file
Binary file not shown.
Binary file modified CompleteVision_MAIN/obj/Debug/Threshold.o
View file
Binary file not shown.
Binary file modified CompleteVision_MAIN/obj/Debug/customblobdetector.o
View file
Binary file not shown.
Binary file modified CompleteVision_MAIN/obj/Debug/main_threaded.o
View file
Binary file not shown.
16 changes: 9 additions & 7 deletions Config.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
#include "LED_Detector.hpp" // for definition of Threshold class
#include <unistd.h>

#define INDOOR
#define OUTDOOR //OUTDOOR //INDOOR // AUTO


/// Camera settings
Expand All @@ -38,12 +38,11 @@ void custom_v4l2_init(void* parm_void)

set_parm(parm->fd, V4L2_CID_C920_AUTOFOCUS,0); // Turn autofocus off
set_parm(parm->fd, V4L2_CID_C920_FOCUSVAL,V4L2_C920_FOCUS_INF); // Use infinity focus (no macro)
set_parm(parm->fd, V4L2_CID_C920_ZOOMVAL,0); // Wide angle zoom
set_parm(parm->fd, V4L2_CID_C920_ZOOMVAL,107); // Zoom (trying to match PS3 Eye)
//set_parm(parm->fd, V4L2_CID_C920_ZOOMVAL,150);
set_parm(parm->fd, V4L2_CID_SATURATION,170); // Adjust Saturation (0-255)
set_parm(parm->fd, V4L2_CID_SHARPNESS,128); // Blur the image to get smoother contours (0-255)

//set_auto_exposure(parm->fd);

#ifdef INDOOR
set_manual_exposure(parm->fd, 20); // Indoor exposure
set_parm(parm->fd, V4L2_CID_GAIN, 255); // Indoor gain
Expand All @@ -54,9 +53,12 @@ void custom_v4l2_init(void* parm_void)
set_parm(parm->fd, V4L2_CID_GAIN, 20); // Outdoor gain
#endif

#ifdef AUTO
set_auto_exposure(parm->fd);
#endif

set_parm(parm->fd, V4L2_CID_BRIGHTNESS, 128);
set_parm(parm->fd, V4L2_CID_CONTRAST, 128);

}


Expand Down Expand Up @@ -96,7 +98,7 @@ void initializeFeatureDetectObj(LED_Detector::Params &params)
params.maxBlobs = 8;

params.sortByColor = true;
params.targetColor = 330; // Red-ish
params.targetColor = 320; // Red-ish
params.filterByColor = true;
params.maxColor = 75;

Expand Down Expand Up @@ -154,7 +156,7 @@ const char* modelPointsFilename =

/// Pose estimate error tolerances
const double POSE_ERR_TOL = 0.05; // if reprojection error is lower than this --> move on
const double SECONDARY_POSE_ERR_TOL = 0.08; // otherwise, try re-ordering LED's and choose lowest
const double SECONDARY_POSE_ERR_TOL = 0.30; // otherwise, try re-ordering LED's and choose lowest
// error that still satisfies the secondary error tolerance

/// Flight data recording
Expand Down
12 changes: 9 additions & 3 deletions FeatureDetector_Test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -188,15 +188,21 @@ void *processing(void*)
{
sleep(3); // Ensure that the capture thread has time to initialize and fill buffer
PnP.is_current = false;

int i_frame = 0;
while(1)
{
++i_frame;


/// TODO: Include some kind of protection to make sure we don't repeatedly process the same frame
/// (if processing somehow goes faster than capturing -- not a problem on BBB right now)
pthread_mutex_lock(&framelock_mutex);
// Decode JPEG image stored in the most recently dequeued buffer
//v4l2_process_image(frame, user_buffer.ptr[user_buffer.buf_last]);
frame = imread("/home/mdarling/Desktop/CompleteVision_MAIN/CompleteVision_MAIN/TestImages_OLD/frame_0032.jpg");
char filepath[200];
sprintf(filepath,"/home/mdarling/Desktop/CompleteVision_MAIN/CompleteVision_MAIN/ManualTests/frame_%04d.jpg",i_frame);
frame = imread(filepath);
Mat roi = frame(Range(400,475),Range(5,450));
roi.setTo(Scalar(255,0,0));

Expand All @@ -208,7 +214,7 @@ while(1)
Detector.findLEDs(frame,gray,binary,imagePoints,DetectorParams,PnP.is_current,PnP.projImagePoints);

// Compute pose estimate
int poseIters = PnP.localizeUAV(imagePoints, poseState, poseErr, 6, POSE_ERR_TOL, SECONDARY_POSE_ERR_TOL); //, preCorrelated);
int poseIters = PnP.localizeUAV(imagePoints, poseState, poseErr, 9, POSE_ERR_TOL, SECONDARY_POSE_ERR_TOL); //, preCorrelated);
if ( poseIters > 0 && checkSanity(poseState) > 0 )
{
PnP.is_current = true;
Expand Down Expand Up @@ -266,7 +272,7 @@ while(1)

PnP.drawOverFrame(frame);
imshow("DEBUG_VIDEO",frame);
waitKey(1);
waitKey(0);
#endif /* DEBUG_VIDEO */


Expand Down
Loading

0 comments on commit 6db7dae

Please sign in to comment.