Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#include "pch_LightBarrier.h"
#include "CameraToWorld.h"
#include "ofxRulr/Exception.h"
#include "ofxRulr/Nodes/Item/Camera.h"
#include "ofxRulr/Nodes/Item/AbstractBoard.h"
#include "ofxCvGui/Widgets/Button.h"
#include "ofxCvGui/Widgets/Toggle.h"
#include "ofxCvGui/Widgets/Slider.h"
#include "ofxCvGui/Widgets/Spacer.h"
#include "ofxCvGui/Widgets/LiveValue.h"
#include "ofxGraycode.h"
using namespace ofxRulr;
using namespace ofxRulr::Nodes;
using namespace ofxRulr::Utils;
using namespace ofxCv;
using namespace cv;
namespace LightBarrier {
#pragma mark Model
//----------
unsigned int CameraToWorld::Model::getParameterCount() const {
return 9;
}
//----------
void CameraToWorld::Model::getResidual(DataPoint dataPoint, double & residual, double * gradient) const {
if (gradient) {
throw(ofxRulr::Exception("CameraToWorld::Model doesn't support gradient based optimisers"));
}
auto evaluated = dataPoint;
this->evaluate(evaluated);
residual = (double)(evaluated.world - dataPoint.world).length();
}
//----------
void CameraToWorld::Model::evaluate(DataPoint & dataPoint) const {
auto parameters = this->getParameters();
if (parameters) {
dataPoint.world.x = parameters[0] * dataPoint.camera.x + parameters[1] * dataPoint.camera.y + parameters[2];
dataPoint.world.y = parameters[3] * dataPoint.camera.x + parameters[4] * dataPoint.camera.y + parameters[5];
dataPoint.world.z = parameters[6] * dataPoint.camera.x + parameters[7] * dataPoint.camera.y + parameters[8];
}
}
#pragma mark CameraToWorld
//----------
CameraToWorld::CameraToWorld() {
RULR_NODE_INIT_LISTENER;
}
//----------
string CameraToWorld::getTypeName() const {
return "LightBarrier::CameraToWorld";
}
//----------
void CameraToWorld::init() {
RULR_NODE_UPDATE_LISTENER;
RULR_NODE_DRAW_WORLD_LISTENER;
RULR_NODE_INSPECTOR_LISTENER;
RULR_NODE_SERIALIZATION_LISTENERS;
auto cameraInput = this->addInput<Item::Camera>();
this->addInput<Item::AbstractBoard>();
this->view = MAKE(ofxCvGui::Panels::World);
this->view->onDrawWorld += [this](ofCamera &) {
auto cameraNode = this->getInput<Item::Camera>();
if (cameraNode) {
cameraNode->drawWorldStage();
}
this->drawWorldStage();
};
auto & camera = this->view->getCamera();
camera.setPosition(ofVec3f(1, -1, 0));
camera.lookAt(ofVec3f(0.0f, 0.0f, 4.0f), ofVec3f(0.0f, -1.0f, 0.0f));
cameraInput->onNewConnection += [this](shared_ptr<Item::Camera> camera) {
auto cameraView = camera->getPanel();
if (cameraView) {
auto cameraWeak = weak_ptr<Item::Camera>(camera);
cameraView->onMouse.addListener([this, cameraWeak](ofxCvGui::MouseArguments & args) {
auto camera = cameraWeak.lock();
if (args.action == ofxCvGui::MouseArguments::Action::Moved && args.isLocal()) {
try {
this->testValue = this->cameraToWorld(args.localNormalized * ofVec2f(camera->getWidth(), camera->getHeight()));
}
catch (...) {
//if the camera has no calibration, then cameraToWorld may throw an exception
//just ignore it
}
}
}, this);
}
};
cameraInput->onDeleteConnection += [this](shared_ptr<Item::Camera> camera) {
if (camera) {
auto cameraView = camera->getPanel();
if (cameraView) {
cameraView->onMouse.removeListeners(this);
}
}
};
this->parameterCallbackEnabled = true;
this->residual = 0.0;
this->cachedCameraWidth = 0.0f;
this->cachedCameraHeight = 0.0f;
}
//----------
void CameraToWorld::serialize(Json::Value & json) {
auto & jsonDataPoints = json["dataPoints"];
int pointIndex = 0;
for (const auto & dataPoint : this->dataPoints) {
auto & jsonDataPoint = jsonDataPoints[pointIndex++];
auto & jsonWorldPoint = jsonDataPoint["world"];
jsonWorldPoint["x"] = dataPoint.world.x;
jsonWorldPoint["y"] = dataPoint.world.y;
jsonWorldPoint["z"] = dataPoint.world.z;
auto & jsonCameraPoint = jsonDataPoint["camera"];
jsonCameraPoint["x"] = dataPoint.camera.x;
jsonCameraPoint["y"] = dataPoint.camera.y;
}
auto & jsonAdditions = json["additions"];
int additionIndex = 0;
for (auto & addition : this->additions) {
jsonAdditions[additionIndex++] = addition;
}
auto & jsonFitParameters = json["fitParameters"];
for (int i = 0; i < 9; i++) {
jsonFitParameters[i] = this->fitParameters[i].get();
}
}
//----------
void CameraToWorld::deserialize(const Json::Value & json) {
this->dataPoints.clear();
const auto & jsonDataPoints = json["dataPoints"];
for (const auto & jsonDataPoint : jsonDataPoints) {
auto & jsonWorldPoint = jsonDataPoint["world"];
DataPoint dataPoint;
dataPoint.world.x = jsonWorldPoint["x"].asFloat();
dataPoint.world.y = jsonWorldPoint["y"].asFloat();
dataPoint.world.z = jsonWorldPoint["z"].asFloat();
auto & jsonCameraPoint = jsonDataPoint["camera"];
dataPoint.camera.x = jsonCameraPoint["x"].asFloat();
dataPoint.camera.y = jsonCameraPoint["y"].asFloat();
this->dataPoints.push_back(dataPoint);
}
this->additions.clear();
const auto & jsonAdditions = json["additions"];
for (const auto & jsonAddition : jsonAdditions) {
this->additions.push_back(jsonAddition.asInt());
}
auto & jsonFitParameters = json["fitParameters"];
this->parameterCallbackEnabled = false;
for (int i = 0; i < 9; i++) {
this->fitParameters[i].set(jsonFitParameters[i].asFloat());
}
this->parameterCallbackEnabled = true;
float dummy;
this->callbackModelParameter(dummy);
this->updatePreview();
}
//----------
ofxCvGui::PanelPtr CameraToWorld::getPanel() {
return this->view;
}
//----------
void CameraToWorld::update() {
auto camera = this->getInput<Item::Camera>();
if (camera) {
if (camera->getWidth() != this->cachedCameraWidth || camera->getHeight() != this->cachedCameraHeight) {
try {
this->updatePreview();
}
RULR_CATCH_ALL_TO_ERROR
}
}
}
//---------
void CameraToWorld::drawWorldStage() {
ofMesh points;
for (auto & point : this->dataPoints) {
points.addVertex(point.world);
points.addColor(ofColor(abs(point.camera.x) * 255.0f, abs(point.camera.y) * 255.0f, 255.0f));
}
Utils::Graphics::pushPointSize(3.0f);
{
points.drawVertices();
}
Utils::Graphics::popPointSize();
ofSphere(this->testValue, 0.05f);
if (this->parameters.showResiduals) {
ofMesh residuals;
residuals.setMode(ofPrimitiveMode::OF_PRIMITIVE_LINES);
for (const auto & dataPoint : this->dataPoints) {
auto evaluated = dataPoint;
this->model.evaluate(evaluated);
residuals.addColor(ofColor(255, 50));
residuals.addVertex(dataPoint.world);
residuals.addColor(ofColor(255, 255));
residuals.addVertex(evaluated.world);
}
residuals.draw();
}
ofPushStyle();
{
ofSetColor(255, 50);
this->planePreview.draw();
}
ofPopStyle();
}
//---------
ofVec3f CameraToWorld::cameraToWorld(const ofVec2f & camera) {
auto cameraNode = this->getInput<Item::Camera>();
if (!cameraNode) {
throw(ofxRulr::Exception("Can't call cameraToWorld"));
}
DataPoint dataPoint;
const auto cameraMatrix = cameraNode->getCameraMatrix();
const auto distortionMatrix = cameraNode->getDistortionCoefficients();
dataPoint.camera = ofxCv::undistortPoint(camera, cameraMatrix, distortionMatrix);
this->model.evaluate(dataPoint);
return dataPoint.world;
}
//----------
void CameraToWorld::populateInspector(ofxCvGui::InspectArguments & inspectArguments) {
auto inspector = inspectArguments.inspector;
inspector->addParameterGroup(this->parameters);
auto clearButton = MAKE(ofxCvGui::Widgets::Button, "Clear", [this]() {
this->additions.clear();
this->dataPoints.clear();
});
clearButton->setHeight(100.0f);
inspector->add(clearButton);
auto findCornersButton = inspector->addButton("Find corners", [this]() {
try {
ScopedProcess scopedProcess("Finding corners");
this->addBoard();
this->fit();
scopedProcess.end();
}
RULR_CATCH_ALL_TO_ERROR;
}, ' ');
findCornersButton->setHeight(100.0f);
inspector->add(MAKE(ofxCvGui::Widgets::Button, "Delete last corners", [this]() {
if (!this->additions.empty()) {
this->dataPoints.resize(MAX(this->dataPoints.size() - this->additions.back(), 0));
this->additions.pop_back();
}
}, OF_KEY_BACKSPACE));
inspector->add(MAKE(ofxCvGui::Widgets::LiveValue<size_t>, "Total correspondences points found", [this]() {
return this->dataPoints.size();
}));
inspector->add(MAKE(ofxCvGui::Widgets::LiveValue<size_t>, "Boards found", [this] {
return this->additions.size();
}));
inspector->add(MAKE(ofxCvGui::Widgets::Spacer));
inspector->add(MAKE(ofxCvGui::Widgets::LiveValue<double>, "Residual [m]", [this]() {
return this->residual;
}));
inspector->add(MAKE(ofxCvGui::Widgets::LiveValue<ofVec3f>, "Test point", [this]() {
return this->testValue;
}));
inspector->add(MAKE(ofxCvGui::Widgets::Spacer));
for (int i = 0; i < 9; i++) {
inspector->add(MAKE(ofxCvGui::Widgets::LiveValue<float>, "Parameter " + ofToString(i), [this, i] {
return this->fitParameters[i];
}));
}
inspector->add(MAKE(ofxCvGui::Widgets::Button, "Update preview", [this]() {
this->updatePreview();
}));
inspector->add(MAKE(ofxCvGui::Widgets::Button, "Set fit to plane", [this]() {
try
{
auto result = ofSystemTextBoxDialog("Plane depth Z (m)", "4.0");
auto depth = ofToFloat(result);
this->setFitToPlane(depth);
}
RULR_CATCH_ALL_TO_ALERT
}));
}
//----------
void CameraToWorld::addBoard() {
this->throwIfMissingAnyConnection();
auto cameraNode = this->getInput<Item::Camera>();
auto boardNode = this->getInput<Item::AbstractBoard>();
vector<cv::Point2f> imagePoints;
vector<cv::Point3f> objectPoints;
auto frame = cameraNode->getGrabber()->getFreshFrame();
auto pixels = frame->getPixels();
auto foundBoard = boardNode->findBoard(ofxCv::toCv(pixels)
, imagePoints
, objectPoints
, this->parameters.findBoardMode
, cameraNode->getCameraMatrix()
, cameraNode->getDistortionCoefficients());
if (!foundBoard) {
throw(ofxRulr::Exception("No corners found"));
}
cout << "Object points " << objectPoints.size() << endl;
cout << "Image points " << imagePoints.size() << endl;
cv::Mat rotation, translation;
cv::solvePnP(objectPoints
, imagePoints
, cameraNode->getCameraMatrix()
, cameraNode->getDistortionCoefficients()
, rotation
, translation
, false);
auto boardTransform = ofxCv::makeMatrix(rotation, translation);
for (int i = 0; i < imagePoints.size(); i++) {
DataPoint dataPoint;
dataPoint.world = (toOf(objectPoints[i]) * boardTransform) * cameraNode->getTransform();
dataPoint.camera = ofxCv::undistortPoint(toOf(imagePoints[i])
, cameraNode->getCameraMatrix()
, cameraNode->getDistortionCoefficients());
this->dataPoints.push_back(dataPoint);
}
this->additions.push_back(imagePoints.size());
}
//----------
void CameraToWorld::setFitToPlane(float depth) {
this->dataPoints.clear();
this->additions.clear();
double parameters[] = { depth, 0.0f, 0.0f,
0.0f, depth, 0.0f,
0.0f, 0.0f, depth };
this->model.setParameters(parameters);
}
//----------
void CameraToWorld::fit() {
ofxNonLinearFit::Fit<Model> fit(ofxNonLinearFit::Algorithm(nlopt::LN_NEWUOA, ofxNonLinearFit::Algorithm::LocalGradientless));
fit.optimise(this->model, &this->dataPoints, &this->residual);
this->residual /= this->dataPoints.size();
this->updateParameters();
this->updatePreview();
}
//----------
void CameraToWorld::callbackModelParameter(float &) {
if (!this->parameterCallbackEnabled) {
return;
}
double values[9];
for (int i = 0; i < 9; i++) {
values[i] = (double) this->fitParameters[i].get();
}
this->model.setParameters(values);
}
//----------
void CameraToWorld::updateParameters() {
this->parameterCallbackEnabled = false;
const auto values = this->model.getParameters();
for (int i = 0; i < 9; i++) {
this->fitParameters[i].set((float)values[i]);
}
this->parameterCallbackEnabled = true;
}
//----------
void CameraToWorld::updatePreview() {
auto camera = this->getInput<Item::Camera>();
if (camera) {
this->planePreview.clear();
this->planePreview.addVertex(this->cameraToWorld(ofVec2f(0, 0)));
this->planePreview.addVertex(this->cameraToWorld(ofVec2f(camera->getWidth(), 0)));
this->planePreview.addVertex(this->cameraToWorld(ofVec2f(camera->getWidth(), camera->getHeight())));
this->planePreview.addVertex(this->cameraToWorld(ofVec2f(0, camera->getHeight())));
this->planePreview.setMode(ofPrimitiveMode::OF_PRIMITIVE_TRIANGLE_FAN);
this->cachedCameraWidth = camera->getWidth();
this->cachedCameraHeight = camera->getHeight();
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.