FileStorage for OpenCV Python API - c++

I'm currently using FileStorage class for storing matrices XML/YAML using OpenCV C++ API.
However, I have to write a Python Script that reads those XML/YAML files.
I'm looking for existing OpenCV Python API that can read the XML/YAML files generated by OpenCV C++ API

You can use PyYAML to parse the YAML file.
Since PyYAML doesn't understand OpenCV data types, you need to specify a constructor for each OpenCV data type that you are trying to load. For example:
import yaml
def opencv_matrix(loader, node):
mapping = loader.construct_mapping(node, deep=True)
mat = np.array(mapping["data"])
mat.resize(mapping["rows"], mapping["cols"])
return mat
yaml.add_constructor(u"tag:yaml.org,2002:opencv-matrix", opencv_matrix)
Once you've done that, loading the yaml file is simple:
with open(file_name) as fin:
result = yaml.load(fin.read())
Result will be a dict, where the keys are the names of whatever you saved in the YAML.

Using the FileStorage functions available in OpenCV 3.2, I've used this with success:
import cv2
fs = cv2.FileStorage("calibration.xml", cv2.FILE_STORAGE_READ)
fn = fs.getNode("Camera_Matrix")
print (fn.mat())

In addition to #misha's response, OpenCV YAML's are somewhat incompatible with Python.
Few reasons for incompatibility are:
Yaml created by OpenCV doesn't have a space after ":". Whereas Python requires it. [Ex: It should be a: 2, and not a:2 for Python]
First line of YAML file created by OpenCV is wrong. Either convert "%YAML:1.0" to "%YAML 1.0". Or skip the first line while reading.
The following function takes care of providing that:
import yaml
import re
def readYAMLFile(fileName):
ret = {}
skip_lines=1 # Skip the first line which says "%YAML:1.0". Or replace it with "%YAML 1.0"
with open(scoreFileName) as fin:
for i in range(skip_lines):
fin.readline()
yamlFileOut = fin.read()
myRe = re.compile(r":([^ ])") # Add space after ":", if it doesn't exist. Python yaml requirement
yamlFileOut = myRe.sub(r': \1', yamlFileOut)
ret = yaml.load(yamlFileOut)
return ret
outDict = readYAMLFile("file.yaml")
NOTE: Above response is applicable only for yaml's. XML's have their own share of problems, something I haven't explored completely.

I wrote a small snippet to read and write FileStorage-compatible YAMLs in Python:
# A yaml constructor is for loading from a yaml node.
# This is taken from #misha 's answer: http://stackoverflow.com/a/15942429
def opencv_matrix_constructor(loader, node):
mapping = loader.construct_mapping(node, deep=True)
mat = np.array(mapping["data"])
mat.resize(mapping["rows"], mapping["cols"])
return mat
yaml.add_constructor(u"tag:yaml.org,2002:opencv-matrix", opencv_matrix_constructor)
# A yaml representer is for dumping structs into a yaml node.
# So for an opencv_matrix type (to be compatible with c++'s FileStorage) we save the rows, cols, type and flattened-data
def opencv_matrix_representer(dumper, mat):
mapping = {'rows': mat.shape[0], 'cols': mat.shape[1], 'dt': 'd', 'data': mat.reshape(-1).tolist()}
return dumper.represent_mapping(u"tag:yaml.org,2002:opencv-matrix", mapping)
yaml.add_representer(np.ndarray, opencv_matrix_representer)
#examples
with open('output.yaml', 'w') as f:
yaml.dump({"a matrix": np.zeros((10,10)), "another_one": np.zeros((2,4))}, f)
with open('output.yaml', 'r') as f:
print yaml.load(f)

To improve on the previous answer by #Roy_Shilkrot I added support for numpy vectors as well as matrices:
# A yaml constructor is for loading from a yaml node.
# This is taken from #misha 's answer: http://stackoverflow.com/a/15942429
def opencv_matrix_constructor(loader, node):
mapping = loader.construct_mapping(node, deep=True)
mat = np.array(mapping["data"])
if mapping["cols"] > 1:
mat.resize(mapping["rows"], mapping["cols"])
else:
mat.resize(mapping["rows"], )
return mat
yaml.add_constructor(u"tag:yaml.org,2002:opencv-matrix", opencv_matrix_constructor)
# A yaml representer is for dumping structs into a yaml node.
# So for an opencv_matrix type (to be compatible with c++'s FileStorage) we save the rows, cols, type and flattened-data
def opencv_matrix_representer(dumper, mat):
if mat.ndim > 1:
mapping = {'rows': mat.shape[0], 'cols': mat.shape[1], 'dt': 'd', 'data': mat.reshape(-1).tolist()}
else:
mapping = {'rows': mat.shape[0], 'cols': 1, 'dt': 'd', 'data': mat.tolist()}
return dumper.represent_mapping(u"tag:yaml.org,2002:opencv-matrix", mapping)
yaml.add_representer(np.ndarray, opencv_matrix_representer)
Example:
with open('output.yaml', 'w') as f:
yaml.dump({"a matrix": np.zeros((10,10)), "another_one": np.zeros((5,))}, f)
with open('output.yaml', 'r') as f:
print yaml.load(f)
Output:
a matrix: !!opencv-matrix
cols: 10
data: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0]
dt: d
rows: 10
another_one: !!opencv-matrix
cols: 1
data: [0.0, 0.0, 0.0, 0.0, 0.0]
dt: d
rows: 5
Though I could not control the order of rows, cols, dt, data.

pip install opencv-contrib-python for video support to install specific version use pip install opencv-contrib-python

Related

How to use the ROS tf listener to get the transform and apply it to my velocity data?

I have velocity data from the sensor, but somehow it is not in the robot body frame. It's been in a world frame, so it's not correct. So I need to transform it in the robot body frame as that how should be in the correct way. So I need a listener that will listen for transformations to be broadcast. Once broadcast I can use the listener to get the transform and apply it to your data.
I made a ROS nodes for broadcast and listener. But is still not correct the velocity. Here is the sensor data that include the linear velocity and its looks like this:
header:
seq: 182
stamp:
secs: 39
nsecs: 244000000
frame_id: "world"
child_frame_id: "rexrov2/base_link"
pose:
pose:
position:
x: -0.0462157448086
y: -0.0175201465699
z: -18.7747349396
orientation:
x: 0.00029722877228
y: -0.000159403004117
z: -0.00433868890864
w: 0.999990530967
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist:
twist:
linear:
x: -0.00672636768741
y: -0.00264084973057
z: 0.204243325029
angular:
x: -7.4723382137e-05
y: -0.00310117164775
z: -0.00108493763869
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Then the robot frame is rexrov2/base_linkand the sensor is rexrov2/pose_sensor_link_default. The frame broadcaster is this one
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf2/LinearMath/Quaternion.h>
void poseCallback(const nav_msgs::Odometry::ConstPtr& msg){
static tf2_ros::TransformBroadcaster br;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "world";
transformStamped.child_frame_id = "rexrov2/base_link";
transformStamped.transform.translation.x = msg->pose.pose.position.x;
transformStamped.transform.translation.y = msg->pose.pose.position.y;
transformStamped.transform.translation.z = msg->pose.pose.position.z;
transformStamped.transform.rotation.x = msg->pose.pose.orientation.x;
transformStamped.transform.rotation.y = msg->pose.pose.orientation.y;
transformStamped.transform.rotation.z = msg->pose.pose.orientation.z;
transformStamped.transform.rotation.w = msg->pose.pose.orientation.w;
br.sendTransform(transformStamped);
}
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf2_broadcaster");
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe("rexrov2/pose_gt", 10, &poseCallback);
ros::spin();
return 0;
};
And the listener node is this one
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf2_listener");
ros::NodeHandle node;
ros::Publisher robot_vel =node.advertise<geometry_msgs::Twist>("robot/vel", 10);
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10.0);
while (node.ok()){
geometry_msgs::TransformStamped transformStamped;
try{
transformStamped = tfBuffer.lookupTransform("rexrov2/base_link", "world", ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
geometry_msgs::Twist vel_msg;
vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2));
robot_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
The sensor data that is not in the correct frame is published under rexrov2/pose_gt and the robot base link is rexrov2/base_link. The warnings I'm getting are:
[ WARN] [1635767328.332529320, 14554.020000000]: "rexrov2/pose_gt" passed to lookupTransform argument target_frame does not exist.
Any help?Thanks
The warning you are getting is because you don't have the TF frames connected. Please check them out with
rosrun rqt_tf_tree rqt_tf_tree
It seems you are not publishing anywhere the transform between "rexrov2/pose_gt" and "rexrov2/base_link". So run this command in a separate terminal to publish the transform so your node can get it:
rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 1 rexrov2/pose_gt resrov2/base_link
If you stop receiving the warning running that command, it means that you need to include it in a launch file along with your node in order to run all at the same time
<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_gt_broadcaster" args="1 0 0 0 0 0 1 resrov2/base_link rexrov2/pose_gt" />
</launch>
And replace the transformation 1 0 0 0 0 0 1 (x y z qx qy qz qw) by the correct one
In addition to this, I don't see any subscriber in your code, but I think it's because you're using the node to do preliminar tests with the ROS integration

Segfault in Tranposed matrix multiplication with .eval() with Eigen 3 library

I added two .eval() just in case. I got no compilation error, and no run time warning. Just segfault.
Thanks for helping me to fix this.
Test:
#include <Eigen/Eigen>
#include <iostream>
using namespace Eigen;
int main() {
Matrix<float, Dynamic, Dynamic> mat_b;
Matrix<float, Dynamic, Dynamic> mat_c;
mat_b << 1.0, 0.0, 0.5, 0.5,
0.0, 1.0, 0.5, 0.5,
1.0, 0.0, 1.0, 0.0,
0.0, 1.0, 0.0, 1.0;
mat_c << 0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
1.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1.0, 0.0, 0.0, 1.0, 0.0, 0.0;
std::cout << (mat_b.transpose().eval() * mat_c).eval() << "\n";
}
Result:
Segmentation fault (core dumped)
As stated in documentatipon
The comma initializer
Eigen offers a comma initializer syntax which allows the user to easily set all the coefficients of a matrix, vector or array. Simply list the coefficients, starting at the top-left corner and moving from left to right and from the top to the bottom. The size of the object needs to be specified beforehand. If you list too few or too many coefficients, Eigen will complain.
emphasis is mine. If you expect that Matrix ctor would deduce size from your formatting, that simply not possible in C++. Looks like you created 16x1 and 24x1 matrix and then try to multiply 1x16 (transposed first one) to 24x1 which is not legal.

Send a matrix as a proc argument in Chapel

I'm getting an error when I try to send a matrix into a proc. I'm pretty sure I'm doing something very wrong, can't figure it out.
use LinearAlgebra;
proc main() {
var A = Matrix(
[0.0, 0.8, 1.1, 0.0, 2.0]
,[0.8, 0.0, 1.3, 1.0, 0.0]
,[1.1, 1.3, 0.0, 0.5, 1.7]
,[0.0, 1.0, 0.5, 0.0, 1.5]
,[2.0, 0.0, 1.7, 1.5, 0.0]
);
check_dims(A);
}
proc check_dims(A: Matrix) {
var t: bool = false;
if (A.domain.dim(1) == A.domain.dim(2)){
t = true;
}
return t;
}
Gives me
mad.chpl:3: In function 'main':
mad.chpl:14: error: unresolved call 'check_dims([domain(2,int(64),false)] real(64))'
mad.chpl:17: note: candidates are: check_dims(A: Matrix)
I'm using chpl Version 1.15.0
Linear algebra objects (like matrices and vectors) are represented as arrays in Chapel. Therefore, changing Matrix (a type that does not exist) to [] (the syntax for array-type) should work as expected:
use LinearAlgebra;
proc main() {
var A = Matrix(
[0.0, 0.8, 1.1, 0.0, 2.0]
,[0.8, 0.0, 1.3, 1.0, 0.0]
,[1.1, 1.3, 0.0, 0.5, 1.7]
,[0.0, 1.0, 0.5, 0.0, 1.5]
,[2.0, 0.0, 1.7, 1.5, 0.0]
);
check_dims(A);
}
proc check_dims(A: []) {
var t: bool = false;
// method is dim()
if (A.domain.dim(1) == A.domain.dim(2)){
t = true;
}
return t;
}

VTK volume rendering of mammography

I am working on vtk volume rendering of mammography. I have a 50 DICOM slices in folder to construct volume. Here I need to give vtkColorTransferFunction and vtkPiecewiseFunction to set RGB color and scalar opacity.
I am not getting exact values of color and opacity with respect to mammo images (breast images). I need values for color and opacity with respect to breast x-ray images.
Any suggestions will be helpful.
vtkGPUVolumeRayCastMapper *volumeGPUmapper =
vtkGPUVolumeRayCastMapper::New();
volumeGPUmapper->SetInputConnection(clip->GetOutputPort());
// RGB and alpha funcions
double skinOnBlueMap[28][5] =
{
{0, 0.987853825092316, 1.0, 1.0, 0.9},
{10000, 0.987853825092316, 1.0, 1.0, 0.9},
{20000, 0.987853825092316, 1.0, 1.0, 1.0},
{30000, 0.987853825092316, 1.0, 1.0, 1.0},
{40000, 0.0, 0.0, 0.0, 1.0},
{50000, 1.0, 0.0, 0.0, 1.0},
{60000, 1.0, 0.999206542968750, 0.0, 1.0},
{70000, 1.0, 1.0, 1.0, 1.0}
};
vtkSmartPointer<vtkPiecewiseFunction> alphaChannelFunc = vtkSmartPointer<vtkPiecewiseFunction>::New();
vtkSmartPointer<vtkColorTransferFunction> colorFunc = vtkSmartPointer<vtkColorTransferFunction>::New();
for(int i = 0; i < sizeof(skinOnBlueMap)/(5*sizeof(double)); i++)
{
colorFunc->AddRGBPoint(skinOnBlueMap[i][0], skinOnBlueMap[i][1], skinOnBlueMap[i][2], skinOnBlueMap[i][3]);
alphaChannelFunc->AddPoint(skinOnBlueMap[i][0], skinOnBlueMap[i][4]);
}
vtkSmartPointer<vtkVolumeProperty> volumeProperty = vtkSmartPointer<vtkVolumeProperty>::New();
volumeProperty->SetColor(colorFunc);
volumeProperty->SetInterpolationTypeToLinear();
volumeProperty->SetScalarOpacity(alphaChannelFunc);
vtkSmartPointer<vtkVolume> VTKvolume = vtkSmartPointer<vtkVolume>::New();
VTKvolume->SetMapper(volumeGPUmapper);
VTKvolume->SetProperty(volumeProperty);
Those scalar values seem strange to me. It could be the reason your results don't look correct. But to be sure we need the image stack and rendered result.
The scalar values are not even inside the range of the unsigned short type, that is generally used for the voxels.

OpenGl - Evaluators and Normals

I'm trying to use evaluators to create a plane:
void Plane::draw(float texS, float texT)
{
float div = v.at(0);
GLfloat ctrlpoints[4][3] = {
{-0.5, 0.0, 0.5}, {-0.5, 0.0 ,-0.5},
{0.5, 0.0, 0.5}, {0.5, 0.0, -0.5}};
GLfloat texturepoints[4][2] = {
{0.0, 0.0}, {0.0, 1.0/texT},
{1.0/texS, 0.0}, {1.0/texS, 1.0/texT}};
glMap2f(GL_MAP2_VERTEX_3, 0.0, 1.0, 3, 2, 0.0, 1.0, 2 * 3, 2, &ctrlpoints[0][0]);
glMap2f(GL_MAP2_TEXTURE_COORD_2, 0.0, 1.0, 2, 2, 0.0, 1.0, 2 * 2, 2, &texturepoints[0][0]);
glEnable(GL_MAP2_VERTEX_3);
glEnable(GL_MAP2_TEXTURE_COORD_2);
glEnable(GL_AUTO_NORMAL);
glMapGrid2f(div, 0.0, 1.0, div, 0.0, 1.0);
glEvalMesh2(GL_FILL,0, div, 0, div);
}
It displays the plane correctly, it gives me a 50*50 grid, for example, and the texture I apply to it is also displayed properly. However, if I try to apply a golden appearance to it, it just gives me a dull brown color.
I know I can get what I want by creating a rectangle with with quad or triangle strip, but the point here is to use evaluators.
One answer I found said that evaluators calculate normals automatically with the enabling of GL_AUTO_NORMAL, and that that was the only necessary instruction. But even then, the author of the question couldn't do what he wanted.
And I do have GL_NORMALIZE enabled in the initialization.