Python Quaternion Calculation Function

I will translate this article to Japanese sooner or later.

This is just for my study and I recommend you to use other well-done python libraries.

Using tf library for handling Quaternion in python

There are already "tf" library to do some kind of these job.
tf (Python) — tf 0.1.0 documentation

Quaternion to Euler angle

For example, you can get euler angle from quaternion q = [x,y,z,w]

import tf

euler = tf.transformations.euler_from_quaternion(q)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]

Euler angle to DCM( Direct Cosine Matrix )

transformations — tf 0.1.0 documentation

Also, you can get 3D rotational matrix from euler angle.

Re = tf.transformations.euler_matrix(roll, pitch, yaw, 'rxyz')

Now ,you can combine upper transformation to get DCM from quaternion!

Test

Let's check this DCM with a simple example.
90 degree rotation around x axis is like below:

q = [1.0/math.sqrt(2) ,0 ,0 , 1.0/math.sqrt]

Then we can get Rotation matrix:

e = tf.transformations.euler_from_quaternion(q)
Re = tf.transformations.euler_matrix(e[0], e[1], e[2], 'rxyz')

Results in,

matrix([[ 1.,  0.,  0.],
        [ 0.,  0., -1.],
        [ 0.,  1.,  0.]])

It convert z axis to -y and y axis to z axis.

My Python Functions

I just need more compact one and write myself for my study.
I use right handed coordinate and scalar part of quaternion is in the 4th element of quaternion.
Like: q = (qx, qy, qz, qw).

I used math class to calculate some sin and cos.

import math

Basic quaternion calculations here: (this is not so compact...)

#### ----- Quaternion Calculation ----- ####

def q_mult(q1, q2):
    x1, y1, z1 ,w1 = q1
    x2, y2, z2 ,w2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
    return x, y, z ,w

def q_inv(q):
    x, y, z ,w = q
    n = x*x + y*y +z*z + w*w
    return [-x/n, -y/n, -z/n, w/n]

# inner product
def q_iprod(q1,q2):
    q = [x*y for (x,y) in zip(q1,q2)] 
    return sum(q)
    
def q_conjugate(q):
    x, y, z ,w = q
    return [-x, -y, -z, w]

def q_normalize(q):
    x, y, z ,w = q
    n = math.sqrt(x*x + y*y +z*z + w*w)
    return [x/n, y/n, z/n, w/n]

# Linear interpolate with rate: [t,(1-t)] (with normalization)     
def q_LERP(q1,q2,t):
    q = [x*(1.0-t)+y*t for (x,y) in zip(q1,q2)]    
    return q_normalize(q)
    
# Sphere interpolate with rate: [t,(1-t)]   
def q_SLERP(q1,q2,t):
    w = math.acos(q_iprod(q1,q2))
    a = math.sin((1.0-t)*w)/ math.sin(w)
    b = math.sin(t*w)/ math.sin(w)
    q = [x*a+y*b for (x,y) in zip(q1,q2)]    
    return q

# Rotate vector using quaternion        
def qv_mult(q1, v1):
    q2 =  v1 + [0.0]
    return q_mult(q_mult(q1, q2), q_conjugate(q1))[:3]

#### ---------------------------------- ####

Remember the scalar part is the 4th element in this definition.
You can just put returning scalar:w to be a first to convert the other formulation.

Quaternion to DCM

DCM can be directly get from quaternion.
Here shows the function:

def q2Rmat(q):
    x,y,z,w = q
    Rmat = np.matrix([ [x*x-y*y-z*z+w*w, 2.0*(x*y-w*z), 2.0*(x*z+w*y) ],\
              [2.0*(x*y+w*z), y*y+w*w-x*x-z*z, 2.0*(y*z-w*x)],\
              [2.0*(x*z-w*y),  2.0*(y*z+w*x), z*z+w*w-x*x-y*y]] )
    return Rmat

Remember the rotational matrix is different depending on vector or coordinate you want to rotate.
This equation is tend to rotate vector.

Test

Let's check this DCM with a simple example.
90 degree rotation around x axis is like below:

q = [1.0/math.sqrt(2) ,0 ,0 , 1.0/math.sqrt]

Then we can get Rotation matrix:

q2Rmat(q)

Results in,

matrix([[ 1.,  0.,  0.],
        [ 0.,  0., -1.],
        [ 0.,  1.,  0.]])

It convert z axis to -y and y axis to z axis.

Comparison Tests

Consider you have orientation of some robot in a quaternion form:

q = [math.sqrt(3)/2 * 0.5 , 0.5*0.5,0,math.sqrt(3)/2] 

then convert to rpy

rpy = [-0.061428016242791844, 0.9815194409145511, -0.08729595308817747]

Using tf.transformation function

array([[ 0.875     ,  0.21650635,  0.4330127 ,  0.        ],
       [ 0.21650635,  0.625     , -0.75      ,  0.        ],
       [-0.4330127 ,  0.75      ,  0.5       ,  0.        ],
       [ 0.        ,  0.        ,  0.        ,  1.        ]])

This output 4 times 4 homogeneous matrix.

Also try my sample,

matrix([[ 0.875     ,  0.21650635,  0.4330127 ],
        [ 0.21650635,  0.625     , -0.75      ],
        [-0.4330127 ,  0.75      ,  0.5       ]])

(for vector rotation)

It is almost the same result, but I think my function is more precise because it directly convert the quaternion.

coding tech memo: zip in python

I thought this kind of description is cool:

q = [x*(1.0-t)+y*t for (x,y) in zip(q1,q2)]    

ROS deliver argument to callback function in subscribers C++/Python

Just a memo.

Reference

How to deliver arguments to a callback function? - ROS Answers: Open Source Q&A Forum
Passing an argument to a subscriber callback [closed] - ROS Answers: Open Source Q&A Forum

Code Example cpp

// in callback
void chatterCallback(const sensor_msgs::Imu::ConstPtr& msg, boost::numeric::ublas::vector<double>& pose)
{
  ROS_INFO("Imu Seq: [%d]", msg->header.seq);
  ROS_INFO("Imu Orientation x: [%f], y: [%f], z: [%f], w: [%f]", msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w); 
  pose(0) += 0.1;
  ROS_INFO("Pose test: [%f]", pose(0));
  
}

// in main()
  // init pose with 0  
  using namespace boost::numeric::ublas;

  vector<double> pose(3);
  ros::Subscriber sub = n.subscribe<sensor_msgs::Imu>("imu/data", 1000, boost::bind(chatterCallback, _1, pose));

Python Example

In python, the code become more simpler:

def callback(data, args): 
  dict_1 = args[0]
  dict_2 = args[1]
... 
sub = rospy.Subscriber("text", String, callback, (dict_1, dict_2))

Passing arguments to callback in Python - ROS Answers: Open Source Q&A Forum

Remember python is always give argument to function with a pointer of container.
By using adequate function, it is able to change arguments from callback function.

Read yaml file with yaml-cpp in ROS C++ / yamlファイルをC++で開く

This article both contains both Japanese and English explanations.
需要とか考えた結果、英語と日本語で書きます。

Goal: Read .yaml calibration files and then publish camera info

以下のようなyamlファイルをC++で読んでPublishするのが目的です。

image_width: 640
image_height: 480
camera_name: narrow_stereo/left
camera_matrix:
  rows: 3
  cols: 3
  data: [438.783367, 0.000000, 305.593336, 0.000000, 437.302876, 243.738352, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.361976, 0.110510, 0.001014, 0.000505, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [0.999978, 0.002789, -0.006046, -0.002816, 0.999986, -0.004401, 0.006034, 0.004417, 0.999972]
projection_matrix:
  rows: 3
  cols: 4
  data: [393.653800, 0.000000, 322.797939, 0.000000, 0.000000, 393.653800, 241.090902, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

And My environment is ROS kinetic and Ubuntu16.04 on amd64 cpu.
環境は上に示したとおりです。

Method: Using yaml-cpp

Installation

Download and make from source. Follow their installation guide.
GitHub - jbeder/yaml-cpp: A YAML parser and emitter in C++

ROS setup (Kinetic)

Here shows how to add dependency on rosmake.
yaml_cpp - ROS Wiki

But I use catkin_make, so I have to change the "package.xml" and "CmakeList" instead.

In your CmakeList in project, you should add

target_link_libraries(${PROJECT_NAME} yaml-cpp)

If you have already added linker, you can write as below:

target_link_libraries(${PROJECT_NAME} <your_already_added_link> yaml-cpp)

Then open your package.xml and add dependency as below:

...
  <build_depend>common_rosdeps</build_depend>
...
  <run_depend>common_rosdeps</run_depend>
...

Now you can edit your project.

Usage

I recommend you to understand what json/yaml format represents and how you read the data in python for introduction.
まずはPythonなどでファイルを開く方法を調べたほうが理解がスムーズに行くと思います。

Sample code

You need to include

#include <yaml-cpp/yaml.h>

and then you can read files like below:

// Load file
YAML::Node lconf = YAML::LoadFile(filename.yaml);

// how to read data
int iw = lconf["image_width"].as<int>();
std::string model = lconf["distortion_model"].as<std::string>();
std::vector<double> distdata = lconf["distortion_coefficients"]["data"].as<std::vector<double>>();

If the type of value is different from you declared, you will see the error about that.

Application: read yaml file and publish camera_info

Here show the sequence I publish these parameter in ros.

    // fill the camera data before running
    sensor_msgs::CameraInfoPtr lcam(new sensor_msgs::CameraInfo());
    sensor_msgs::CameraInfoPtr rcam(new sensor_msgs::CameraInfo());

    // read calib file
    std::string fnamel,fnamer;
    
    if( argc != 3 ){ // argc should be 2 for correct execution
        fnamel = "/home/ubuntu/calibres/left1.yml";
        fnamer = "/home/ubuntu/calibres/right1.yml";
        std::cout << "Load defaults! " <<  std::endl;
    }else {
        fnamel=argv[1];
        fnamer=argv[2];
        std::cout << "Now load..." << " " << fnamel << " and " << fnamer << std::endl;
    }
    
    YAML::Node lconf = YAML::LoadFile(fnamel);
    YAML::Node rconf = YAML::LoadFile(fnamer);
    
    // start read left camera configure! 
    lcam->width = lconf["image_width"].as<int>();
    lcam->height = lconf["image_height"].as<int>();
    lcam->distortion_model = lconf["distortion_model"].as<std::string>();    
    // get value
    std::vector<double> dd,kk,rr,pp,buf;
    dd = lconf["distortion_coefficients"]["data"].as<std::vector<double>>();
    kk = lconf["camera_matrix"]["data"].as<std::vector<double>>();
    rr = lconf["rectification_matrix"]["data"].as<std::vector<double>>();
    pp = lconf["projection_matrix"]["data"].as<std::vector<double>>();
    // conversion
    boost::array<double, 9ul> kl;
    std::memcpy(&kl[0], &kk[0], sizeof(double)*9);
    boost::array<double, 9ul> rl;
    std::memcpy(&rl[0], &rr[0], sizeof(double)*9);
    boost::array<double, 12ul> pl;
    std::memcpy(&pl[0], &pp[0], sizeof(double)*12);
    // put value
    lcam->D =  dd; 
    lcam->K =  kl; 
    lcam->R =  rl; 
    lcam->P =  pl;

Method: Using OpenCV builtin function

There is a good hint for another solution
sourishghosh.com

How to split string using multiple delimeters / and convert to double array in C++

GOAL: get double array from "[1.1, 3.3]"

Unfortunately std::string do not have split function.

I've heard boost/ library has split function but I do not want to use that.

Split string into string vector

Here is sample code.
The original code is from below:
c++ - Split a string into words by multiple delimiters - Stack Overflow

#include <iostream>
#include <string>
#include <vector>

int main(){
std::string str = "[0.0, 2.1, 22.3]";
   std::vector<std::string> v ; //Use vector to add the words

    std::size_t prev_pos = 0, pos;
    while ((pos = str.find_first_of("[] ,", prev_pos)) != std::string::npos)
    {
        if (pos > prev_pos)
            v.push_back(str.substr(prev_pos, pos-prev_pos));
        prev_pos= pos+1;
    }
    if (prev_pos< str.length())
        v.push_back(str.substr(prev_pos, std::string::npos));


std::cout << v << std::endl;
std::cout << v[0] << std::endl;
std::cout << v[1] << std::endl;

}

Out put become:

[0.0, 2.1, 22.3]
0.0
2.1

Give it try

You can try this code in CodePad.

C++ code - 21 lines - codepad

Convert string vector to vector of double

Just using while loop. It will be safer.
Code is like below:

#include <iostream>
#include <string>
#include <vector>

int main(){
std::string str = "[0.0, 2.1, 22.3]";

// copied from stackoverflow
std::vector<std::string> v ; //Use vector to add the words

    std::size_t prev_pos = 0, pos;
    while ((pos = str.find_first_of("[] ,", prev_pos)) != std::string::npos)
    {
        if (pos > prev_pos)
            v.push_back(str.substr(prev_pos, pos-prev_pos));
        prev_pos= pos+1;
    }
    if (prev_pos< str.length())
        v.push_back(str.substr(prev_pos, std::string::npos));


std::cout << v[1] << std::endl;
std::cout << v.size() << std::endl;

double* num;
num = new double[v.size()];

for(unsigned int i=0;i<v.size();i++){
    num[i] = std::stod(v[i]);
} 

std::cout << num[1] << std::endl;

}

try?

Codepad could not compile this due to compiler dependency or some error.
There is another page to test this code.
Online C++ Compiler - online editor

Written as a Function

#include <iostream>
#include <string>
#include <vector>

void string2doublearray(std::string str, std::string delim, double *&num){
    
    std::vector<std::string> v ; //Use vector to add the words

    std::size_t prev_pos = 0, pos;
    while ((pos = str.find_first_of("[] ,", prev_pos)) != std::string::npos)
    {
        if (pos > prev_pos)
            v.push_back(str.substr(prev_pos, pos-prev_pos));
        prev_pos= pos+1;
    }
    if (prev_pos< str.length())
        v.push_back(str.substr(prev_pos, std::string::npos));

    num = new double[v.size()];

    for(unsigned int i=0;i<v.size();i++){
        num[i] = std::stof(v[i]);
    } 
    
    std::cout<< num[1] << std::endl;
}

int main(){
std::string str = "[0.0, 2.1, 22.3]";
std::string delim = "[] ,";
double *d;

string2doublearray(str,delim,d);

std::cout<< d[1] << std::endl;

}

ROS publish the camera info in C++ (PerceptIn Camera)

I'm now using PerceptIn camera to make ROS wrapper.

Publish camera image

See the sample code online_feature.cpp to check how to get image from perceptIn camera.

Publish Camera info

Initialization and put value

Use cameraInfo.h.
sensor_msgs/CameraInfo Documentation
First, include and add your value.

Part of an example is below:

//init 
sensor_msgs::CameraInfoPtr lcam(new sensor_msgs::CameraInfo());
// put value
lcam->height = 480;
lcam->width = 640;
lcam->distortion_model = "plumb_bob";
lcam->D = { -0.361976, 0.110510, 0.001014, 0.000505, 0.000000};
lcam->K = {438.783367, 0.000000, 305.593336, 0.000000, 437.302876, 243.738352, 0.000000, 0.000000, 1.000000};
lcam->R = {0.999978, 0.002789, -0.006046, -0.002816, 0.999986, -0.004401, 0.006034, 0.004417, 0.999972};
lcam->P = {393.653800, 0.000000, 322.797939, 0.000000, 0.000000, 393.653800, 241.090902, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000};

Of course, it is better to read a calibration file to fill these information.

Publishing

When you publish the data, you should synchronize the camera image and camera info (many ROS packages yield that).

Sample code is like below:

lcam->header.stamp = imageLeftMsg->header.stamp;
lcam->header.frame_id = imageLeftMsg->header.frame_id;

imageLeftMsg means image data is for data to be published.

When debugging

Be careful [rostopic echo] will not visualize your CameraInfo until the synchronized image is subscribed by another topic.
github.com

Python,c++でyamlファイル読み込み

jsonに引き続きyamlファイルの読み込みをメモ。

Python

以下の通り。
How can I parse a YAML file in Python - Stack Overflow

import ruamel.yaml as yaml

with open("example.yaml") as stream:
    try:
        print(yaml.load(stream))
    except yaml.YAMLError as exc:
        print(exc)

C++

今回はOpencvにおいてROSの保存したキャリブレーションファイルを読み込みたいので
一番目の方法を採用します。

opencvを使って真面目に読む方法

cv::FileNodeというクラスを使って頑張って読めるようです。
c++ - Read data from yaml file issue - opencv - Stack Overflow

yaml++.hを使う

こちらの一番上の解決策を使います。
Parse YAML Files in C++ - Stack Overflow

yaml.cppを使う

インストールしてビルドする必要がありますが、一番シンプルにかけそうです。
使用例がないので日和って使いませんでしたが。

GitHub - jbeder/yaml-cpp: A YAML parser and emitter in C++

料理初心者でもできる？白ワインとチキンブロスで作る簡単リゾット

というわけで実際に簡単飯？を試してみました。

元レシピ

レシピは以下のサイトからとってきました。
ちゃんと作るなら下のサイトを見た方が安全です。
おいしいチーズリゾット – おいしいアメリカ：アメリカ料理レシピと食べ歩き情報満載！

材料について

元レシピで材料は

• チキンストック 1½カップ (360cc)
• 水 1½カップ (360cc)
• バター 大さじ2
• たまねぎ 1個(みじん切り)
• アーボリオ米 1カップ
• 白ワイン 1/2カップ (120cc)
• パルメジャンチーズ 1/2-1カップ(削ったもの)
• 塩こしょう

と言われています。チキンストックとは、平たく言うと液状のコンソメの元的なSomethingのはずです。
チキンブロスとかブイヨンとか仲間が多くてあまり区別がついていないですが。

すべて比較的簡単に手に入るはずですがバターとかワインとか代用がきくのではないでしょうか。

買い出し

コメを7kg買って持って帰るのがきつかった...

買った材料

• チキンブロス 1缶 (350ccくらい？)
• 玉ねぎ： 拳くらい 1個
• バター：1/5ブロック（つまり適当）
• 米：250ccくらい コップ一杯
• 白ワイン：適当
• モッツァレラチーズ：好きなだけ

チキンブラストブロスはこんなやつを買いました。100円いかないくらい。

ワインも安いやつ（500円未満）を買って飲んだり料理したりします。

調理開始

大体通しで20分くらいかかります。
「強火」で「5分」とか言われても知るかって感じなので見た目ベースで記録しています。悪しからず。

玉ねぎを切り、フライパンで炒める

玉ねぎを一個まるまるみじん切りにします。
どうせ加熱すると小さくなるのでサイズは適当でいいです。

そのあと、鍋にバターを入れて炒めます。
私は麻雀配1.5個分くらい入れたと思います。
バターは玉ねぎと米にしみこむので多少多い分には問題ないです。

「中火で5分」炒めるそうですがまぁ玉ねぎが半透明になってしんなりするまで炒めればいいんです。うん。

米を投入し半透明になるまで炒める

タイトルの通り米を投入して炒めます。投入前にあまりに鍋がパッサパサならちょっと油なりバターを足してもよさげな気がします。
入れすぎるとたぶん油が跳ねます。たぶん。

生米は白濁していますが、それが透明になるまで炒めます。目安は中火で3分らしいです。

ワインを投入しさらに炒める。

大体の米粒が透明になったら（何粒かはかたくなに白いので無視）コップ1/3くらいのワインをいれて炒めます。
ジュっ！！！って音がします。割とビビる。

この工程そんな重要なのかな？とちょっと思っちゃう。
香り付けとか風味づけとかそういう高尚な行程なのでしょう。

水気が再び飛ぶまで炒めます。２分くらい？

チキンブロスを半分くらい投入。ぐつぐつ煮る。

先ほどのワインの汁気が飛んだらチキンブロスを入れます。
なければ水＆コンソメとかで代用してください。

はじめ蓋なしでやっていましたが速攻で水気が飛んでやばかったのできちんと蓋をしましょう。
中火で5～10分くらい煮ます。焦げ付かないように時々開けてかき混ぜるのがいいでしょう。（焦がした人）

残りのチキンブロスをちまちま投入、水気を飛ばす。

蓋を開けると水気が減ってリゾット感が出ている頃かと思います。

ちょっと米をつまんでみて「ちょっと芯が残っているけどまぁかみ砕けるかな」くらいになっていれば仕上げです。
残りのチキンブロスを3回くらいに分けて入れて「水気が飛ぶ」→「入れてかき混ぜる」を繰り返して冷まして完了・

チーズとか胡椒とかお好みで

モッツアレラチーズしかよく知らないので刻んだモッツアレラチーズを入れましたがちょっと粘り気が強すぎる印象。
ネッチョリしないチーズ（パルミジャーノとか？）のほうが良いかもしれません。

きちんと火が通ってさえいればOKなのであとは塩と胡椒で味付けして完成。

見た目は良くないけどまぁまぁです。

ただ，三人前くらいできます。食べきれないのでホストのお婆ちゃんに少しあげました。

トータルコスト

お金：350円くらい

米：多分300g行かないくらい？=50円強
ワイン：1/8 * 500 = 60円強
バター：1/20 * 600 = 30円強
玉ねぎ：30円くらい
チーズ：1/5 * 300 = 60円くらい
チキンブロス：80円くらい

• • • • • • • • • • • • • • • -

合計：350円くらい

まぁまぁ安い。日本で作っても悪くないくらいの値段です。

時間：けっこうかかる

ただ，買い物でさまよった時間＋調理とちょっとした片付けで2時間半かかりました。
この記事書いて3時間半です。これは休日とかにしか頑張れない...

補足

正直見た目が寂しいので玉ねぎを炒める工程で刻んだマッシュルームやアスパラガスを。
コメを炒めた後あたりにトマトとか入れても美味しそう。試してみたら更新します。

今後これを続けるなら手順を簡略化したいですね。

チキンブロスが無い時

コンソメでもブイヨンでもブロスでもなんでもいいですが，要は出汁を切らした場合を先程体験しました。
結論やや淡白な味わいになりますが食べれないことはないです。

代わりにケチャップとチリソース（お店からかっぱらってきた）があったのでそれとトマトを入れて作りました。
顆粒タイプの何らかの出汁があればベストですがなければ醤油やケチャップで十分味付け可能のように感じます。
バターやチーズが風味を出してくれるのもでかいのかも。

とにかく，ケチャップは何かと使えるし現地で買っておいて損はないかと思います。チキンライスも作れるだろうし。