Fail to use ANN_MLP in Opencv 3 in Java - java

I tried using ANN_MLP in OpenCV 3.0 to train a model for a simple XOR operation (i.e. 00->0, 01->1, 10->1, 11->0). But it returned NaN when I called nn.predict. What's wrong with the code? Here is my Java code:
package jm.app;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.TermCriteria;
import org.opencv.ml.ANN_MLP;
import org.opencv.ml.Ml;
import org.opencv.ml.StatModel;
public class Main {
static{System.loadLibrary(Core.NATIVE_LIBRARY_NAME);}
public static void main(String[] args) {
Mat trainData = new Mat(4, 2, CvType.CV_32FC1);
trainData.put(0, 0, 0);
trainData.put(0, 1, 0);
trainData.put(1, 0, 0);
trainData.put(1, 1, 1);
trainData.put(2, 0, 1);
trainData.put(2, 1, 0);
trainData.put(3, 0, 1);
trainData.put(3, 1, 1);
Mat trainLabels = new Mat(4, 1, CvType.CV_32FC1);
trainLabels.put(0, 0, 0);
trainLabels.put(1, 0, 1);
trainLabels.put(2, 0, 1);
trainLabels.put(3, 0, 0);
ANN_MLP nn = ANN_MLP.create();
nn.setActivationFunction(ANN_MLP.SIGMOID_SYM);
nn.setTrainMethod(ANN_MLP.BACKPROP);
nn.setBackpropMomentumScale(0.1);
nn.setBackpropWeightScale(0.1);
nn.setTermCriteria(new TermCriteria(TermCriteria.MAX_ITER, (int)100000, 0.000001));
Mat layers = new Mat(1, 3, CvType.CV_32SC1);
layers.put(0, 0, 2);
layers.put(0, 1, 3);
layers.put(0, 2, 1);
nn.setLayerSizes(layers);
nn.train(trainData, Ml.ROW_SAMPLE, trainLabels);
Mat testData = new Mat(1, 2, CvType.CV_32FC1);
testData.put(0, 0, 1);
testData.put(0, 1, 1);
Mat testLabels = new Mat(1, 1, CvType.CV_32FC1);
float res = nn.predict(testData, testLabels, ANN_MLP.RAW_OUTPUT);
Util.printMat(testLabels);
Mat layer1 = nn.getWeights(0);
Mat layer2 = nn.getWeights(1);
Mat layer3 = nn.getWeights(2);
Util.printMat(layer1);
Util.printMat(layer2);
Util.printMat(layer3);
}
}
package jm.app;
import org.opencv.core.Mat;
public class Util {
public static void printMat(Mat mat){
System.out.println();
System.out.print(mat.rows() + " * " + mat.cols());
for(int i = 0; i < mat.rows(); i++){
System.out.println();
for(int j = 0; j < mat.cols(); j++){
System.out.print(mat.get(i, j)[0] + " ");
}
}
System.out.println();
}
}
And the output is:
1 * 1
NaN
1 * 4
2.0 -1.0 2.0 -1.0
3 * 3
-0.417962425638773 -0.11805564491195578 0.7527567170648859
0.40930192249590086 -0.24876980957807385 -0.2929439299929529
0.6025307693048867 0.2936134607392147 -0.10605986687856579
4 * 1
0.5558049015443158
0.4766362469511742
0.3713056187114578
-0.24058588929784652
So I have two questions:
1. Why the "testLabel" got a NaN value?
2. Why the "layer1" is a 1*4 matrix? What did the "layer1" do here?

Related

How to get multipliers after solving a quadratic program in ojAlgo

I implement a Sequential quadratic programming (SQP) optimizer and use ojAlgo for the quadratic programming (QP) subproblem.
My question is:
How do I get hold of the "Lagrange multipliers" for the QP solution?
In the attached example code that solve an QP result.getMultipliers() only return an empty Optional.
package com.mycompany.testojalgo;
import java.math.BigDecimal;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.ojalgo.matrix.Primitive64Matrix;
import org.ojalgo.optimisation.Expression;
import org.ojalgo.optimisation.ExpressionsBasedModel;
import org.ojalgo.optimisation.Optimisation;
import org.ojalgo.optimisation.Variable;
import org.ojalgo.structure.Access1D;
import org.ojalgo.type.StandardType;
import org.ojalgo.type.context.NumberContext;
public class ojAlgoQP {
public static void main(String[] args) {
testOjAlgoQuadraticProgramming();
}
public static void testOjAlgoQuadraticProgramming() {
// QP Example 16.2 p453 in 'Numerical Optimization', 2ed, (2006), Jorge Nocedal and Stephen J. Wright.
// minimize function F(x1,x2,x3) = 3*x1*x1 + 2*x1*x2 + x1*x3 + 2.5*x2*x2 + 2*x2*x3 + 2*x3*x3 - 8*x1 - 3*x2 - 3*x3
// x = [x1, x2, x3]'
// F(x) = 1/2*x'*H*x + x'*g
// constraints x1 + x3 = 3, x2 + x3 = 0
// A*x = b
//objectiveGradient
Primitive64Matrix g = Primitive64Matrix.FACTORY.rows(new double[][]{
{-8}, {-3}, {-3}
});
//objectiveHessian
Primitive64Matrix H = Primitive64Matrix.FACTORY.rows(new double[][]{
{6, 2, 1},
{2, 5, 2},
{1, 2, 4}
});
Variable x1 = new Variable("x1");
Variable x2 = new Variable("x2");
Variable x3 = new Variable("x3");
// constraint equations
Primitive64Matrix A = Primitive64Matrix.FACTORY.rows(new double[][]{
{1, 0, 1},
{0, 1, 1}
});
// required constraint values
Primitive64Matrix b = Primitive64Matrix.FACTORY.rows(new double[][]{
{3}, {0}
});
List<Variable> variables = new ArrayList<>();
variables.add(x1);
variables.add(x2);
variables.add(x3);
ExpressionsBasedModel model = new ExpressionsBasedModel(variables);
Expression energy = model.addExpression("Energy");
energy.setLinearFactors(variables, g);
//divide by two to express function using hessian.
energy.setQuadraticFactors(variables, H.divide(2));
energy.weight(BigDecimal.ONE);
//create constraint equations
for (int i = 0; i < A.countRows(); i++) {
Expression expression = model.addExpression("Constraint#"+i);
for (int j = 0; j < A.countColumns(); j++) {
expression.set(variables.get(j), A.get(i, j));
}
expression.level(b.get(i));
}
Optimisation.Result result = model.minimise();
NumberContext accuracy = StandardType.PERCENT.withPrecision(1);
boolean ok = model.validate(result, accuracy);
Optimisation.State v = result.getState();
// How do I get the multipliers
Optional<Access1D<?>> multipliers = result.getMultipliers();
double value1 = result.getValue();
// Get result and check value and constraint
Primitive64Matrix x = Primitive64Matrix.FACTORY.rows(new double[][]{
{x1.getValue().doubleValue()}, {x2.getValue().doubleValue()}, {x3.getValue().doubleValue()}
});
//divide by two to express function using hessian, again.
Primitive64Matrix value = x.transpose().multiply(H.divide(2)).multiply(x).add(x.transpose().multiply(g));
Primitive64Matrix residual= A.multiply(x).subtract(b);
}
}
Update 1:
Here is my reworked example using org.ojalgo.optimisation.convex.ConvexSolver.getBuilder();
package com.mycompany.testojalgo;
import java.util.Optional;
import org.ojalgo.matrix.store.MatrixStore;
import org.ojalgo.matrix.store.Primitive64Store;
import org.ojalgo.optimisation.Optimisation;
import org.ojalgo.optimisation.convex.ConvexSolver;
import org.ojalgo.structure.Access1D;
public class ojAlgoQP {
public static void main(String[] args) {
testOjAlgoQuadraticProgramming2();
}
public static void testOjAlgoQuadraticProgramming2() {
// QP Example 16.2 p453 in 'Numerical Optimization', 2ed, (2006), Jorge Nocedal and Stephen J. Wright.
// minimize function F(x1,x2,x3) = 3*x1*x1 + 2*x1*x2 + x1*x3 + 2.5*x2*x2 + 2*x2*x3 + 2*x3*x3 - 8*x1 - 3*x2 - 3*x3
// x = [x1, x2, x3]'
// F(x) = 1/2*x'*H*x + x'*g
// constraints x1 + x3 = 3, x2 + x3 = 0
// A*x = b
//objectiveGradient
Primitive64Store gStore = Primitive64Store.FACTORY.rows(new double[][]{
{-8}, {-3}, {-3}
});
//objectiveHessian
Primitive64Store HStore = Primitive64Store.FACTORY.rows(new double[][]{
{6, 2, 1},
{2, 5, 2},
{1, 2, 4}
});
// constraint equations
Primitive64Store AStore = Primitive64Store.FACTORY.rows(new double[][]{
{1, 0, 1},
{0, 1, 1}
});
// required constraint values
Primitive64Store bStore = Primitive64Store.FACTORY.rows(new double[][]{
{3}, {0}
});
ConvexSolver.Builder builder = ConvexSolver.getBuilder();
builder.equalities(AStore, bStore);
builder.objective(HStore, gStore.negate());
ConvexSolver solver = builder.build();
Optimisation.Result result = solver.solve();
// How do I get the multipliers? multipliers = Optional.empty
Optional<Access1D<?>> multipliers = result.getMultipliers();
// value1 = -3.5
double value1 = result.getValue();
// Verify result:
// x= [2.0, -0.9999999999999996, 0.9999999999999997]';
// value = -3.5
// residual =[-4.440892098500626E-16, 1.1102230246251565E-16]'
Primitive64Store x = Primitive64Store.FACTORY.column(result.toRawCopy1D());
MatrixStore<Double> value = x.transpose().multiply(HStore.multiply(0.5)).multiply(x).add(x.transpose().multiply(gStore));
MatrixStore<Double> residual = AStore.multiply(x).subtract(bStore);
}
}
I believe that is an Optional because it was (sometimes) too messy to map the Lagrange multipliers from the solver to the constraints of the model.
If you're implementing an SQP solver may I suggest that you don't implement it in terms of ExpressionsBasedModel, but delegate to the convex solvers directly. Build something that implements org.ojalgo.optimisation.Optimisation.Solver and delegate to the various classes in the org.ojalgo.optimisation.convex package. Then you code more directly with the matrices, vectors and multipliers.
To make that solver usable by ExpressionsBasedModel you also implement an org.ojalgo.optimisation.Optimisation.Integration and register that by calling ExpressionsBasedModel.addPreferredSolver(myIntegeration) or ExpressionsBasedModel.addFallbackSolver(myIntegeration).
Implementing a solver and making it usable from the modelling tool are two separate things.

Moka7 - Java - Single bit Read - Write

Please, can you post an example to set/write a single bit on a DB ?
With this i write entire byte of the DB (i suppose)
ClientPlc.ReadArea(S7.S7AreaDB, 200, 0, 1, Buffer);
Thanks a lot
byte[] data = new byte[2];
res = client.ReadArea(S7.S7AreaDB, 59001, 0, 2, data); //I want to read DB7.DBD0 (it is a real)
if (res == 0) {
retEmerg01 = S7.GetBitAt(data, 0, 0);
retEmerg02 = S7.GetBitAt(data, 0, 1);
retEmerg03 = S7.GetBitAt(data, 0, 2);
retEmerg04 = S7.GetBitAt(data, 0, 3);
retEmerg05 = S7.GetBitAt(data, 0, 4);
retEmerg06 = S7.GetBitAt(data, 0, 5);
retEmerg07 = S7.GetBitAt(data, 0, 6);
retEmerg08 = S7.GetBitAt(data, 0, 7);
retEmerg09 = S7.GetBitAt(data, 1, 0);
retEmerg10 = S7.GetBitAt(data, 1, 1);
retEmerg11 = S7.GetBitAt(data, 1, 2);
retEmerg12 = S7.GetBitAt(data, 1, 3);
retEmerg13 = S7.GetBitAt(data, 1, 4);
}
You can do so.
For me it works like this:
void WriteBit(int miDB, int miOffset, int miBit, boolean miVal){
if (Connected){
try{
byte[] dataWrite = new byte[1];
Client.ReadArea(S7.S7AreaDB, miDB, miOffset, 1, dataWrite);
//Keeping adjacent values
S7.SetBitAt(dataWrite, 0, miBit, miVal);
Client.WriteArea(S7.S7AreaDB, miDB, miOffset, 1,dataWrite);
}catch(Exception e){println(e);}
}
}
boolean ReadBit(int miDB, int miOffset, int miBit){
boolean miVal = false;
if (Connected){
byte[] dataRead = new byte[1];
Client.ReadArea(S7.S7AreaDB, miDB, miOffset, 1, dataRead);
miVal =S7.GetBitAt(dataRead, 0, miBit);
return miVal;
}
else return false;
}

Machine learning on a 8 x 8 matrix - Java

I am trying to implement a machine learning algorithm (k-nn for example).As it stands my Main class, which essentially builds 8×8-pixel matrices into an array to be manipulated later. (See the data description and sample dataset.) As it stands my arrays are printing as a like so:
, Class Code: 7 DataSet:[0, 0, 3, 9, 15, 8, 0, 0, 0, 1, 15, 16, 16, 7, 0, 0, 0, 0, 5, 16, 16, 10, 4, 0, 0, 0, 3, 16, 16, 16, 9, 0, 0, 0, 0, 15, 14, 4, 0, 0, 0, 0, 0, 13, 5, 0, 0, 0, 0, 0, 1, 15, 3, 0, 0, 0, 0, 0, 4, 13, 0, 0, 0, 0, 7]
Now for my starting point I'm looking to try to implement a very basic kNN algorithm as something to build from but I am having trouble manipulating the datasets that are being outputted. I have been reading up on Foundations of Machine Learning by M. Mohri but it hasn't been of any help. My Main class for building my data:
import java.util.*;
import java.io.*;
public class Main {
static class Data {
int[] dataSet;
int classCode;
public Data(int[] dataSet, int label) {
this.dataSet = dataSet;
this.classCode = label;
}
#Override
public String toString() {
return "Class Code: " + classCode + " DataSet:" + Arrays.toString(dataSet) + "\n";
}
}
ArrayList<Data> dataSetList;
int[][] dataArray = new int[2810][65];
private void readFile(String csvFile) {
int instances = 0;
dataSetList = new ArrayList<>();
try {
Scanner scan = new Scanner(new BufferedReader(new FileReader(csvFile)));
while (scan.hasNext()) {
String line = scan.next();
String[] extractedDataFromFile = line.split(",");
for (int i = 0; i < extractedDataFromFile.length; i++) {
dataArray[instances][i] = Integer.parseInt(extractedDataFromFile[i]);
}
dataSetList.add(new Data(dataArray[instances], dataArray[instances][extractedDataFromFile.length - 1]));
instances++;
}
System.out.println(dataSetList.toString());
} catch (FileNotFoundException ex) {
System.out.println(ex.getMessage());
}
}
public static void main(String[] args) {
Main main = new Main();
main.readFile("dataset1.csv");
}
}
This is my first time experimenting with machine learning so any feedback or approach to this would be hugely appreciated.
EDIT//
I'm looking at a basic kNN implementation as a starting point whether someone can redirect me to material on implementing on a similar data set or an example using my current provided code. I apologize if my initial post was a little vague

Create an OpenCV Mat with simple pattern

In OpenCV, is there a fast way to create a Mat object where:
odd columns are '1'
even columns are '0'
For example :
1 0 1 0 1 0
1 0 1 0 1 0
1 0 1 0 1 0
The pattern is always the same.
The size of Mat can be big, and process by looping is really slow to generate this pattern.
OpenCV repeat is there exactly for this.
#include <opencv2\opencv.hpp>
#include <vector>
using namespace std;
using namespace cv;
int main(int argc, char** argv)
{
int rows = 1000;
int cols = 1000;
vector<uchar> pattern = { 1, 0 }; // change with int, double, etc according to the type you want.
Mat m;
repeat(pattern, rows, cols/2, m);
return 0;
}
COMPARISON WITH OTHER METHODS
Just a small test to measure the performance of the proposed (so far) methods:
Time in milliseconds:
#Miki [repeat] : 0.442786
#RonaldoMessi [copyTo] : 7.26822
#Derman [merge] : 1.17588
The code I used for the test:
#include <opencv2\opencv.hpp>
#include <vector>
#include <iostream>
using namespace std;
using namespace cv;
int main(int argc, char** argv)
{
int rows = 1000;
int cols = 1000;
{
// #Miki
double tic = (double)getTickCount();
vector<uchar> pattern = { 1, 0 };
Mat m1;
repeat(pattern, rows, cols / 2, m1);
double toc = ((double)getTickCount() - tic) * 1000 / getTickFrequency();
cout << "#Miki [repeat] \t\t: " << toc << endl;
}
{
// #RonaldoMessi
double tic = (double)getTickCount();
Mat m2(rows, cols, CV_8UC1);
Mat vZeros = Mat::zeros(rows, 1, CV_8UC1);
Mat vOnes = Mat::ones(rows, 1, CV_8UC1);
for (int i = 0; i < cols - 1; i += 2)
{
vOnes.col(0).copyTo(m2.col(i));
vZeros.col(0).copyTo(m2.col(i + 1));
}
double toc = ((double)getTickCount() - tic) * 1000 / getTickFrequency();
cout << "#RonaldoMessi [copyTo] \t: " << toc << endl;
}
{
// #Derman
// NOTE: corrected to give correct output
double tic = (double)getTickCount();
Mat myMat[2];
myMat[0] = cv::Mat::ones(rows, cols/2, CV_8UC1);
myMat[1] = cv::Mat::zeros(rows, cols/2, CV_8UC1);
Mat m3;
merge(myMat, 2, m3);
m3 = m3.reshape(1);
double toc = ((double)getTickCount() - tic) * 1000 / getTickFrequency();
cout << "#Derman [merge] \t: " << toc << endl;
}
getchar();
return 0;
}
You can create two column vectors vZeros and vOnes, then copy these columns to the matrix M:
int cols = A.cols;
int rows = A.rows;
Mat vZeros = Mat::zeros(rows , 1, CV_64F);
Mat vOnes = Mat::ones(rows , 1, CV_64F);
for(int i=0; i<cols-1; i+=2)
{
vOnes.col( 0 ).copyTo( M.col(i) );
vZeros.col( 0 ).copyTo( M.col(i+1) );
}
If two-channel matrix won't bother you, this could be your choice:
int rows = 5;
int cols = 5;
cv::Mat myMat[2];
myMat[0] = cv::Mat::ones(rows, cols, CV_32FC1);
myMat[1] = cv::Mat::zeros(rows, cols, CV_32FC1);
cv::Mat result;
cv::merge(myMat, 2, result);
And this is your result:
[1, 0, 1, 0, 1, 0, 1, 0, 1, 0;
1, 0, 1, 0, 1, 0, 1, 0, 1, 0;
1, 0, 1, 0, 1, 0, 1, 0, 1, 0;
1, 0, 1, 0, 1, 0, 1, 0, 1, 0;
1, 0, 1, 0, 1, 0, 1, 0, 1, 0]

How to implement the main() method while using Processing in Java?

I tried to implement the following processing code in a JFrame in netbeans.
code was taken from http://www.geekmomprojects.com/mpu-6050-dmp-data-from-i2cdevlib/
However after running MyFrame.java the it only shows a still image in the frame.
It is not taking the serial readings.
MyProcessingSketch.java
`package testprocessing;
/*
* To change this license header, choose License Headers in Project Properties.
* To change this template file, choose Tools | Templates
* and open the template in the editor.
*/
import java.util.Arrays;
import processing.core.*;
import processing.serial.*;
import processing.opengl.*;
import toxi.geom.*;
import toxi.processing.*;
public class MyProcessingSketch extends PApplet {
ToxiclibsSupport gfx;
Serial port; // The serial port
char[] teapotPacket = new char[14]; // InvenSense Teapot packet
int serialCount = 0; // current packet byte position
int synced = 0;
int interval = 0;
float[] q = new float[4];
Quaternion quat = new Quaternion(1, 0, 0, 0);
float[] gravity = new float[3];
float[] euler = new float[3];
float[] ypr = new float[3];
#Override
public void setup() {
// 300px square viewport using OpenGL rendering
size(300, 300, OPENGL);
gfx = new ToxiclibsSupport(this);
// setup lights and antialiasing
lights();
smooth();
// display serial port list for debugging/clarity
System.out.println(Arrays.toString(Serial.list()));
// get the first available port (use EITHER this OR the specific port code below)
//String portName = Serial.list()[0];
// get a specific serial port (use EITHER this OR the first-available code above)
String portName = "COM10";
// open the serial port
port = new Serial(this, portName, 9600);
// send single character to trigger DMP init/start
// (expected by MPU6050_DMP6 example Arduino sketch)
port.write('r');
}
#Override
public void draw() {
if (millis() - interval > 1000) {
// resend single character to trigger DMP init/start
// in case the MPU is halted/reset while applet is running
port.write('r');
interval = millis();
}
// black background
background(0);
// translate everything to the middle of the viewport
pushMatrix();
translate(width / 2, height / 2);
// 3-step rotation from yaw/pitch/roll angles (gimbal lock!)
// ...and other weirdness I haven't figured out yet
//rotateY(-ypr[0]);
//rotateZ(-ypr[1]);
//rotateX(-ypr[2]);
// toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!)
// (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of
// different coordinate system orientation assumptions between Processing
// and InvenSense DMP)
float[] axis = quat.toAxisAngle();
rotate(axis[0], -axis[1], axis[3], axis[2]);
// draw main body in red
fill(255, 0, 0, 200);
box(10, 10, 200);
// draw front-facing tip in blue
fill(0, 0, 255, 200);
pushMatrix();
translate(0, 0, -120);
rotateX(PI/2);
drawCylinder(0, 20, 20, 8);
popMatrix();
// draw wings and tail fin in green
fill(0, 255, 0, 200);
beginShape(TRIANGLES);
vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); // wing top layer
vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); // wing bottom layer
vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // tail left layer
vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); // tail right layer
endShape();
beginShape(QUADS);
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80);
vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80);
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30);
vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98);
vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70);
vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70);
endShape();
popMatrix();
}
void serialEvent(Serial port) {
interval = millis();
while (port.available() > 0) {
int ch = port.read();
if (synced == 0 && ch != '$') return; // initial synchronization - also used to resync/realign if needed
synced = 1;
print ((char)ch);
if ((serialCount == 1 && ch != 2)
|| (serialCount == 12 && ch != '\r')
|| (serialCount == 13 && ch != '\n')) {
serialCount = 0;
synced = 0;
return;
}
if (serialCount > 0 || ch == '$') {
teapotPacket[serialCount++] = (char)ch;
if (serialCount == 14) {
serialCount = 0; // restart packet byte position
// get quaternion from data packet
q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f;
q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f;
q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f;
q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f;
for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];
// set our toxilibs quaternion to new data
quat.set(q[0], q[1], q[2], q[3]);
/*
// below calculations unnecessary for orientation only using toxilibs
// calculate gravity vector
gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
// calculate Euler angles
euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]);
euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1);
// calculate yaw/pitch/roll angles
ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
// output various components for debugging
//println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f);
//println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI);
//println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI);
*/
}
}
}
}
void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) {
float angle = 0;
float angleIncrement = TWO_PI / sides;
beginShape(QUAD_STRIP);
for (int i = 0; i < sides + 1; ++i) {
vertex(topRadius*cos(angle), 0, topRadius*sin(angle));
vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle));
angle += angleIncrement;
}
endShape();
// If it is not a cone, draw the circular top cap
if (topRadius != 0) {
angle = 0;
beginShape(TRIANGLE_FAN);
// Center point
vertex(0, 0, 0);
for (int i = 0; i < sides + 1; i++) {
vertex(topRadius * cos(angle), 0, topRadius * sin(angle));
angle += angleIncrement;
}
endShape();
}
// If it is not a cone, draw the circular bottom cap
if (bottomRadius != 0) {
angle = 0;
beginShape(TRIANGLE_FAN);
// Center point
vertex(0, tall, 0);
for (int i = 0; i < sides + 1; i++) {
vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
angle += angleIncrement;
}
endShape();
}
}
public static void main(String[] args) {
PApplet.main(new String[] { "--present", "MyProcessingSketch" });
}
}
`
MyFrame.java
package testprocessing;
import javax.swing.JFrame;
public class MyFrame extends JFrame{
private MyProcessingSketch mysketch;
public MyFrame() {
setTitle("IMU");
setDefaultCloseOperation(EXIT_ON_CLOSE);
mysketch = new MyProcessingSketch();
mysketch.init();
add(mysketch);
}
public static void main(String[] args) {
MyFrame frame = new MyFrame();
frame.pack();
frame.setVisible(true);
}
}

Categories