Skip to content

Commit

Permalink
updated wrapper with getter/ setter functions and attributes
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamuelDudley
SamuelDudley committed Feb 4, 2017
1 parent 10f88d2 commit e305706
Show file tree
Hide file tree
Showing 3 changed files with 136 additions and 70 deletions.
13 changes: 8 additions & 5 deletions Examples/Monocular/XIMEA.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,23 +65,26 @@ Viewer.ViewpointF: 2000
#--------------------------------------------------------------------------------------------
# Marker Parameters
#--------------------------------------------------------------------------------------------
Marker.size: 0.287
Marker.dictionary: "ARUCO_MIP_36h12"
Marker.track: 0 # attempt to track markers in each frame: 1 or 0
Marker.data:
-
name: takeoff
id: 100
dictionary: "ARUCO_MIP_36h12"
size: 0.287
-
name: nav
name: land
id: 101
dictionary: "ARUCO_MIP_36h12"
size: 0.287
-
name: land
name: nav
id: 102
dictionary: "ARUCO_MIP_36h12"
size: 0.287


-
name: nav
id: 103
dictionary: "ARUCO_MIP_36h12"
size: 0.287
48 changes: 44 additions & 4 deletions include/Wrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,22 +24,60 @@
#include<boost/python.hpp>
#include<xiApiPlusOcv.h>


struct MarkerData
{
MarkerData() :
name(), dict(), id(), ssize(-1)
{
}

string name;
string dict;
int id;
float ssize;


void read(const cv::FileNode& node) //Read serialization for this class
{
name = (string)node["name"];
dict = (string)node["dictionary"];
id = (int)node["id"];
ssize = (float)node["size"];
}
};

static void read(const cv::FileNode& node, MarkerData& x, const MarkerData& default_value = MarkerData()){
if(node.empty())
x = default_value;
else
x.read(node);
}

class Wrapper
{
public:
void configure(std::string strVocFile, std::string strConfigFile);
Wrapper(std::string strVocFile, std::string strConfigFile); // wrapper constructor
void initialize();
void shutdown();
void configure(std::string strConfigFile);
void track();
int getStatus();
void reset();
void getCurrentFrame();
bool getTrackMarkers();
void setTrackMarkers(bool shouldTrackMarkers);
bool getIsInitialized();

public:
std::string msg;
std::string vocabularyFilePath;
std::string configurationFilePath;

bool isInitialized;
bool trackMarkers;


xiAPIplusCameraOcv cam;
cv::Mat src;
cv::Mat im;
Expand All @@ -56,15 +94,17 @@ class Wrapper

aruco::CameraParameters cameraParameters;
aruco::MarkerDetector markerDetector;
std::map<uint32_t,aruco::MarkerPoseTracker> markerTracker; // use a map so that for each id, we use a different pose tracker
std::map<uint32_t, aruco::MarkerPoseTracker> markerTracker; // use a map so that for each id, we use a different pose tracker
vector<aruco::Marker> markers;
float markerSize;
std::map<uint32_t, MarkerData> markerConfigurations;

private:
bool initialized();

private:
ORB_SLAM2::System* SLAM;


};


#endif // WRAPPER_H
145 changes: 84 additions & 61 deletions src/Wrapper.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,73 +12,50 @@ using namespace std;
using namespace aruco;
namespace python = boost::python;


struct MarkerData
{
MarkerData() :
name(), dictionary(), id(), ssize(-1)
{
}

string name;
string dictionary;
int id;
float ssize;


void read(const cv::FileNode& node) //Read serialization for this class
{
name = (string)node["name"];
dictionary = (string)node["dictionary"];
id = (int)node["id"];
ssize = (float)node["size"];
}
};

static void read(const cv::FileNode& node, MarkerData& x, const MarkerData& default_value = MarkerData()){
if(node.empty())
x = default_value;
else
x.read(node);
}

void Wrapper::configure(string strVocFile, string strConfigFile)
Wrapper::Wrapper(string strVocFile, string strConfigFile)
{
configurationFilePath = strConfigFile; // "./../Examples/Monocular/XIMEA.yaml";
vocabularyFilePath = strVocFile; // "./../Vocabulary/ORBvoc.bin";
configure(strConfigFile);
isInitialized = false;
}



//Check configuration file
void Wrapper::configure(string strConfigFile)
{
// Check configuration file
cv::FileStorage fsConfiguration(configurationFilePath.c_str(), cv::FileStorage::READ);
if(!fsConfiguration.isOpened())
{
cerr << "Failed to open configuration file at: " << configurationFilePath << endl;
exit(-1);
}

// http://stackoverflow.com/questions/7940216/parsing-nested-yml-file-at-second-node
// Get marker configuration info
cv::FileNode marker_info = fsConfiguration["Marker.data"];
cout << "attempting to read marker data" << endl; //Show default behavior for empty matrix
cout << "attempting to read marker data" << endl;

for(cv::FileNodeIterator fit = marker_info.begin(); fit != marker_info.end(); ++fit)
{
string name = (string)(*fit)["name"];
string dictionary = (string)(*fit)["dictionary"];
int id = (int)(*fit)["id"];
float ssize = (float)(*fit)["size"];
// string name = (string)(*fit)["name"];
// string dict = (string)(*fit)["dictionary"];
// int id = (int)(*fit)["id"];
// float ssize = (float)(*fit)["size"];

MarkerData m;
fit >> m;
//
cout << id << dictionary << endl;
cout << m.id << m.ssize << endl;
// cout << someval << endl;
}
*fit >> m;

// map insert will not change the value if the key already exists, the [] operator will.
// As we want to overwrite any existing values on configuration yaml reload we use
// use the [] operator

markerConfigurations[m.id] = m;

cout << m.id << " : " << m.ssize << "m" << endl;
}

// arUco config

// arUco configuration
cameraMatrix = (cv::Mat_<float>(3,3) << fsConfiguration["Camera.fx"] , 0, fsConfiguration["Camera.cx"],
0, fsConfiguration["Camera.fy"], fsConfiguration["Camera.cy"],
0, 0, 1);
Expand All @@ -87,18 +64,17 @@ void Wrapper::configure(string strVocFile, string strConfigFile)

cameraParameters.setParams(cameraMatrix, distorsionCoeff, cv::Size(fsConfiguration["Camera.width"],fsConfiguration["Camera.height"]));

markerSize = fsConfiguration["Marker.size"];

markerDetector.setThresholdParams(7, 7);
markerDetector.setThresholdParamRange(2, 0);

markerDetector.setDictionary(fsConfiguration["Marker.dictionary"], 0.f);
setTrackMarkers(static_cast<int>(fsConfiguration["Marker.track"]));

fsConfiguration.release();
}

void Wrapper::initialize()
{
// Create SLAM system. It initializes all system threads and gets ready to process frames.
SLAM = new ORB_SLAM2::System(vocabularyFilePath, configurationFilePath, ORB_SLAM2::System::MONOCULAR, true);
// XIMEA OpenCV
cam.OpenFirst();
cam.StartAcquisition();
Expand All @@ -109,59 +85,106 @@ void Wrapper::initialize()

cam.SetAutoExposureAutoGainExposurePriority(0.8f);

// Create SLAM system. It initializes all system threads and gets ready to process frames.
SLAM = new ORB_SLAM2::System(vocabularyFilePath, configurationFilePath, ORB_SLAM2::System::MONOCULAR, true);

isInitialized = true;
}

void Wrapper::shutdown()
{
if (!initialized()) {
return;
}
SLAM->Shutdown();
exit(1);
}

bool Wrapper::getIsInitialized()
{
return isInitialized;
}

bool Wrapper::initialized()
{
if (!isInitialized) {
cerr << "System is not initialized " << endl;
return false;
} else {
return true;
}
}

bool Wrapper::getTrackMarkers()
{
return trackMarkers;
}

void Wrapper::setTrackMarkers(bool shouldTrackMarkers)
{
trackMarkers = shouldTrackMarkers;
}

void Wrapper::track()
{
if (!initialized()) {
return;
}

src = cam.GetNextImageOcvMat();
cv::resize(src, im, cv::Size(1024,1024), 0, 0, cv::INTER_CUBIC); // resize image from camera (ideally we would be doing binning on the camera here)
cout << "FPS = " << cam.GetFrameRate() << endl;

// Detect markers
markers = markerDetector.detect(im);
// Do pose estimation for each marker
for(auto & marker:markers) {
markerTracker[marker.id].estimatePose(marker,cameraParameters,markerSize);
if (trackMarkers) {
// Detect markers
markers = markerDetector.detect(im);
// Do pose estimation for each marker
for(auto & marker:markers) {
markerTracker[marker.id].estimatePose(marker,cameraParameters, markerConfigurations[marker.id].ssize);
}
}


SLAM->TrackMonocular(im, tframe, autopilotPoseCurrent, autopilotPoseCurrent, markers);
}

int Wrapper::getStatus()
{
if (!initialized()) {
return -1;
}
cout << "status: " << SLAM->GetStatus() << endl;
return SLAM->GetStatus();
}

void Wrapper::getCurrentFrame()
{
if (!initialized()) {
return;
}
currentFrame = SLAM->GetFrameDrawer()->DrawFrame();
// return currentFrame;
// return currentFrame; // TODO: need cv::Mat to numpy.array converter for this
}

void Wrapper::reset()
{
if (!initialized()) {
return;
}
SLAM->Reset();
}

BOOST_PYTHON_MODULE(libSLAM)
{
using namespace python;
class_<Wrapper>("Wrapper")
.def("configure", &Wrapper::configure)
.def("getStatus", &Wrapper::getStatus)
class_<Wrapper>("Wrapper", init<std::string, std::string>()) // constructor for Wrapper class
.add_property("status", &Wrapper::getStatus) // read-only
.add_property("currentFrame", &Wrapper::getCurrentFrame) // read-only
.add_property("trackMarkers", &Wrapper::getTrackMarkers, &Wrapper::setTrackMarkers) // read / write
.add_property("isInitialized", &Wrapper::getIsInitialized) // read-only
.def("shutdown", &Wrapper::shutdown)
.def("track", &Wrapper::track)
.def("reset", &Wrapper::reset)
.def("initialize", &Wrapper::initialize)
.def("getCurrentFrame", &Wrapper::getCurrentFrame)
;
}

0 comments on commit e305706

Please sign in to comment.