[Programming C++] From shared_ptr to shared_ptr maintaining the use_count

Sometimes we need to store shared data pointers in an anoymous shared pointer shared_ptr<void>. The problem is when you want to convert back the shared_ptr<void> to shared_ptr<T> again. A priori, it is not possible, but we can do some tricks to solve this issue. Neddless to say that the first solution is – of course –  NOT USE shared_ptr<void>. Try to use boost::any or boost::variant objects in your problem. But still, if there may be some strange cases where it can be interesting the use of  anonymous shared_ptrs

The most you can until the momment is getting the original data is using the shared_ptr<void>::get() method to get the void* pointer. However you must maintain the shared_ptr<void> alive if you want to avoid the segmentation fault. The following test would FAIL in the last line:

TEST(basic_test, original_from_shared_void_to_t)
std::shared_ptr<A> a(new A);
std::shared_ptr<void> b =a;





printf(“Testing access memory…\n”);
A* data=(A*)b.get();
data->foo(); //you can access to the fields and methods
//OK by the moment..



//and now crash because memory has been freed…

data->foo(); //segmentation fault

Do you have any idea? Perhaps you are thinking in this one (but it does not work):

std::shared_ptr<A> data= std::shared_ptr<A>((A*)b.get());
data->foo(); //it works because b still references it

//but the problem is the next fact
//both have different counts, they both should be 2 instead 1
//so here comes the crash…

//the destructor is called

lesson lernt: you have to maintain b (or a copy of it) alive while the new pointer “data” is alive.

How to do it without think about it continuing working as always? The  solution is the “__magic_reinterpret_pointer_cast”. see the code:

template<typename T>
std::shared_ptr<T> __magic_reinterpret_pointer_cast(std::shared_ptr<void> b)
return std::shared_ptr<T>((T*)b.get(),[b](T* x){});

Whit it you can go back to your type shared_ptr<A> from shared_ptr<void> avoiding any segmentation fault. The trick is store a reference of the shared_ptr<void> inside the new shared_ptr<A>. Observe that there will be different “use counts” groups, so the use_count method is not reliable anymore. Appart from this everything look work well: See this example and please make me know other limitations on the approach if you notice it.

NOTE: Be careful because it has the same limitations to the reinterpet_cast. ¡¡This is not valid to cast to base or derived types!!. TEST(basic_test,t1)

TEST(basic_test, original_from_shared_void_to_t_reintrepret_cast)

std::shared_ptr<A> a(new A);
std::shared_ptr<void> b=a;


//removing one instance

//restoring it again

//”make magic trick
std::shared_ptr<A> c=__magic_reinterpret_pointer_cast<A>(b);



//”make magic trick
std::shared_ptr<A> c=__magic_reinterpret_pointer_cast<A>(b);



ASSERT_TRUE(c.use_count()==1); // VOILA, STILL 1!


[Programming c#] Use structs instead classes in a loops

Structs in c# enable the development of efficient pieces of code where the managed dynamic memory is not required. Structs in .net are value types. That means that they can be allocated in the stack efficiently instead of the heap. Thanks to this you can have a chunk of data that acts like an object (it has fields and methods) but avoiding the call of the memory management system (garabage collection, that may even call to the operative system to reserve more memory).

For instance, let the simple struct A:

		struct A
			public int val;

			public A(int v)

			public void foo()

The following sample shows the function “bar” that uses the struct “A” intensively in a loop (a typical scenario where efficiency may be very relevant).

         void bar()
             for(int i=0;i<1000;i++)
                 A a;

The use of A as an struct instead a class has three main benefits:

  1.  First a construction function call is avoided. Avoiding function calls in loops may boost the process. For instance, function calls typically decreases the performance of the processor dynamic scheduling or even because of static optimization techniques like performed by the compiler or JITTER (such as loop unrolling).
  2. It is not required to call the dynamic memory management system. That means that the memory management system in .net does not need to create a new node for the object. Moreover any call to the operative system to reserve more memory for the process is totally avoided.
  3.  No garbage collection is required at all since the struct is not in the memory heap. The “deallocation of the object” is automatically done (with no processing cost) when the current function returns back to the calling function (change of stack frame).

However, sometimes it is required to make some kind of initialization process for the struct. Because of this c# structs can be initialization using a parametric constructor. Structs constructors are invoked using the “new” keyword. That may be confusing because the “new” keyword has been historically used in other languages (such as c++) as a method to reserve dynamic memory. If the constructor is called, the benefit 1 is lost, however the benefits 2 and 3 are still maintained.

         void bar()
             for(int i=0;i<1000;i++)
                 A a = new A(4);

Quick logistic function with python and scipy

Basic example about how to make work the logistic function (or whichever else in scipy). It may be interesting to remember.

> ipython -pylab

import scipy
import scipy.stats
(x,y)=map(list, zip(*[ (i,d.cdf(i)) for i in numpy.arange(-1.0,1.0,0.01)]))

Eclipse CDT – Memory Heap limit

Para los sufridores del indexador de autocompletado de eclipse CDT: Ya sabréis que eclipse acaba cascando cuando intentade indexar unos cuantos de directorios de headers. Se puede solucionar facilmente aumentando la cantidad de memoria virtual posible que puede utilizar la jvm que hospeda al proceso eclipse.

Si tenéis la versión bundle ejecutar directamente:

./eclipse -vmargs -XX:MaxPermSize=600m

Mano de santo.


Interesting Survey about different Robotic Software Frameworks for Distributed Robotics

This work identifies many aspects of the distributed robotics and compare ROS and other Robotic Frameworks for distributed robotics. Here I show the abstract:


Robotics is an area of research in which the paradigm of Multi-Agent Systems (MAS) can prove to be highly useful. Multi-Agent Systems come in the form of cooperative robots in a team, sensor networks based on mobile robots, and robots in Intelligent Environments, to name but a few. However, the development of Multi-Agent Robotic Systems (MARS) still presents major challenges. Over the past decade, a high number of Robotics Software Frameworks (RSFs) have appeared which propose some solutions to the most recurrent problems in robotics. Some of these frameworks, such as ROS, YARP, OROCOS, ORCA, Open-RTM, and Open-RDK, possess certain characteristics and provide the basic infrastructure necessary for the development of MARS. The contribution of this work is the identification of such characteristics as well as the analysis of these frameworks in comparison with the general-purpose Multi-Agent System Frameworks (MASFs), such as JADE and Mobile-C.


  • Robotics;
  • MAS;
  • Agents;
  • Software frameworks;
  • Middleware;
  • Architecture

Cambiando el marco de referencia de una distribución Gaussiana

El trabajo de funciones probabilísticas gausianas en el espacio es algo muy habitual en robótica. Este tipo de distribuciones permite expresar una posición o pose (por ejemplo de un objeto u objetivo) de la cual se tiene cierta incertidumbre. Desde mi punto de vista se trata de una temática bastante interesante y me está permitiendo profundizar en algunos conceptos matemáticos como autovalores, números complejos y quaterniones. Otros posts previos que realicé en el pasado con temáticas relacionadas son:

Una función gausiana está compuesta de su vector media \mu y su matriz de covarianza \Sigma y se denota como P(x)\sim\mathcal{N}(\mu,\Sigma). El problema consiste en modificar estos parámetros para pasar del marco de referencia F_{A} al marco de referencia F_{B}. Estos marcos de referencia están descritos por una transformación lineal que incluye rotación y translación respecto a un marco de referencia universal, de modo que:



Cambio del marco de referencia del vector media \mu

El cambio de referencia del vector media es trivial dada una transformación TF entre marcos de referencia básicamente consiste en aplicar la transformación lineal al vector \mu_{A}:

\mu_{B} = R \mu_{A} + T



De echo en ocasiones los valores universales de los marcos de referencia R_{A}, T_{A}, R_{B}  y T_{B} no son conocidos, sino que directamente la relación entre ambos marcos TF\equiv (R,T).

Cambio del marco de referencia de la matriz de Covarianza \Sigma

¿Pero que pasa con la matriz de covarianza? En el caso de un problema unidimensional, el cambio de referencia del origen de coordenadas no afectaría a la matriz de covarianza \Sigma=\begin{bmatrix} \sigma^{2} \end{bmatrix} (que es la varianza). Por ejemplo: P(x)\sim\mathcal{N}(\mu_{A}=3,\Sigma_{A}=5) se convertiría en P(x)\sim\mathcal{N}(\mu_{B}=-7,\Sigma_{B}=5) si cambiaramos el origen de coordenadas desde el O=0 a O=10.

Para una función gausiana 2D o 3D es mucho más complicado. La matriz de covarianza representa la orientación de la distribución además de la  escala de la variabilidad en dentro de los ejes principales de esta rotación. A la derecha se muestra una figura de una función gausiana 2D. En una función gaussiana (creo, a ver si un matemático me lo confirma) que la probabilidad se extiende en N vectores ortogonales (autovectores) con diferentes escalas (autovalores) verificando que:

\Sigma = E (l I) E^{t}

Siendo l el vector de autovalores, el punto curioso es que la matriz E es una matriz de rotación cuyas columnas son los autovectores de \Sigma.  Es intuitivo ver que si el marco de referencia cambia, las coordenadas de estos autovectores cambiará, sin embargo la escala (autovalores) se mantendrá constante. Parece entonces sencillo.

Si estamos trabajando en el marco de referencia F_{A}F_{B} es conocido, entonces basta con cambiar el marco de referencia de los autovectores:



o lo que es lo mismo si tenemos la información completa de F_{A}F_{B}:


Casos particulares que aún no se resolver

Sin embargo existen casos particulares y estos son los que a mi, personalmente me inquietan y aún no tengo claro como resolver:

  1. ¿Qué ocurre si la matriz de covarianza no es diagonalizable? Por ejemplo, se tiene total certeza de su posición en X, su posición en Y y algo de incertidumbre en Z. Siendo \Sigma_{A}=\begin{bmatrix}  0 & 0 & 0\\  0 & 0 & 0\\  0 & 0 & 2.3  \end{bmatrix} ¿Podría obtenerse la matriz E_{A} si solo tenemos un autovector?
  2. ¿Qué ocurre cuando trabajamos (en lugar de con un vector “posición” en 3D) con una “pose en el espacio” xyz+rpy 6D con una matriz de covarianza de 36 valores, donde se mezclan distintas unidades? Este tipo de representación gausiana es también muy utilizada en robótica, sin embargo, el abordaje del problema de cambio de sistema referencia de este tipo de distribuciones es algo que no he logrado localizar en ningún sitio. Parece un caso muy extraño dado que además los valores de rpy dependen en sí mismos del los ejes x,y,z del marco de referencia actual.
Ideas problema 1:
En el ejemplo tratado parece claro que el único autovector existente es v_0=\begin{vmatrix}\\ 0\\ 0\\ 1 \end{vmatrix} y su autovector \lambda_0=2.3 ya que verifica que \Sigma v_0 = \lambda v_0 . Intuyo que dada la precondición de ortogonalidad de los autovectores de la matriz E(¿inventado?) parece razonable que solo necesitamos un autovector para calcular E. De hecho  cualquier E que verifique que l=\begin{vmatrix}\\ \lambda_0\\ \lambda_1=0\\\lambda_2=0 \end{vmatrix} y sea ortogonal parece suficiente ¿sera correcto este razonamiento?Duda: Pienso que una matriz de covarianza no nula debería tener al menos un autovector real, pero de esto, no estoy seguro. Si fuera así, el problema estaría resuelto.
Ideas problema 2:
Creo que dado que el cambio de coordenadas en robótica solo afectaría a los ejes x, y y z el problema se simplificaría. El vector \mu para x, y y z cambiaría de la misma forma. Por el contrario los valores rpy en lugar de ser rotados serían trasladados. \mu_{B}= R* \mu_{A} + T donde:
R=\begin{bmatrix}  R_{xyz} & \O \\  \O & I\\\end{bmatrix}
T=\begin{bmatrix} t_x \\ t_y \\ t_z \\ getRoll(R) \\ getPitch(R) \\ getYaw(R) \\\end{bmatrix}
De esta forma las componentes de \Sigma_A definidas por covarianzas angulares, como \sigma_{row,pitch}=cov(row,pitch) permanecerían constantes tras la transformación TF, mientras que las componentes de la matríz como \sigma_{x,yaw}=cov(x,yaw) cambiarían su valor. Sin embargo, todo esto son por ahora solo intuiciones. ¿es correcto este razonamiento?¿Alguna ayuda?

Basic Rotations with Quaternions

The quaternions is a very interesting mathematical tools that make possible perform rotation operations without using rotation matrices. This method is efficent and secure (avoiding the gimbal lock effect) in dynamic situations. A very interesting explanation about the mathematical foundations and capacities of quaternions can be found here.

The ROS library TF provide several functions to handle quaternions in C++ and Python. There exists many opensource libraries which provides quaternion funciontalities like Eigen, Bullet to name a couple of them.

Rotating a point
ROS works with the right hand axis convention. This axis convention is specially adequate for mobile robots. Here the robot is always looking to the axis-x infinity.

import roslib
import tf
from tf.transformations import *

# we want to rotate the point using the x-axis ("roll")

# the object is located in the y=1. We use the format [x,y,z,w] and w is allways 0 for vectors
vec= [0,1,0,0]

#now we apply the mathematical operation res=q*v*q'. We use this function for multiplication but internally it is just a complex multiplication operation.
result=quaternion_multiply(quaternion_multiply(rot, vec),quaternion_conjugate(rot))

In [33]: result
Out[33]: array([ 0. , 0.70710678, -0.70710678, 0. ])

Concatenate Rotations
Here I show how to concatenate multiple rotations using quaternions:


#the full transform is q4*q3*q2*q1
rot= quaternion_multiply(q4,quaternion_multiply(q3,quaternion_multiply(q2,q1)))
result=quaternion_multiply(quaternion_multiply(rot, vec),quaternion_conjugate(rot))

In [86]: result1
Out[86]: array([ 0. , 0.70710678, -0.70710678, 0. ])

In [90]: result2
Out[90]: array([ 0., 1., 0., 0.])

In [93]: result3
Out[93]: array([ 0. , 0.70710678, -0.70710678, 0. ])

In [96]: result
Out[96]: array([-0.70710678, 0.70710678, 0. , 0. ])

Starting with rosjava and image messages

This post is a mini-tutorial based on the basic rosjava tutorials whose code can be found in [https://rosjava.googlecode.com/hg/) Revision c53f30aa6fb6: /]

Firstly you must known the main concepts of ROS: nodes, topics, messages, etc. After this I recommend you to install the latest rosjava_core stack: http://www.ros.org/wiki/rosjava/Tutorials/Install. Then, to begin with rosjava is also recommendable to have a look to http://www.ros.org/wiki/rosjava/Overview.

After this you should make sure that you have compiled the java source code for the sensor_msgs/Image. To do this, you should understand before how the rosjava_bootstrap works. This package makes possible the java source code generation for any kind of messages. You can realize if you have the sensor message compiled checking if you have the folder for sensor_msgs:

cd  rospack find rosjava_bootstrap/target/classes/org/ros/message/sensor_msgs.

If not, as it was my case then you should generate the source code. We must compile the most essential message types for the correct building of rosjava: std_msgs, sensor_msgs and rosgraph_msgs

geus@geus-vaio:/opt/ros/external/packages/rosjava_core/rosjava_bootstrap$ rosrun rosjava_bootstrap java_msgs.py std_msgs sensor_msgs rosgraph_msgs geometry_msgs -o src/main/java/
generating messages for package [std_msgs]
generated messages for package [std_msgs] to [src/main/java/org/ros/message/std_msgs]
generating messages for package [sensor_msgs]
generated messages for package [sensor_msgs] to [src/main/java/org/ros/message/sensor_msgs]
generating messages for package [rosgraph_msgs]
generated messages for package [rosgraph_msgs] to [src/main/java/org/ros/message/rosgraph_msgs]
generated messages for package [geometry_msgs] to [src/main/java/org/ros/message/geometry_msgs]

Check if the java sourcecode is compiled:

geus@geus-vaio:/opt/ros/external/packages/rosjava_core/rosjava_bootstrap$ ls src/main/java/org/ros/message/
Duration.java  Message.java   rosgraph_msgs/ sensor_msgs/   Service.java   std_msgs/      Time.java

As we can see the sensor_msgs is available:

geus@geus-vaio:/opt/ros/external/packages/rosjava_core/rosjava_bootstrap$ ls src/main/java/org/ros/message/sensor_msgs/
CameraInfo.java      CompressedImage.java  Imu.java         JoyFeedbackArray.java  Joy.java        NavSatFix.java     PointCloud2.java  PointField.java  RegionOfInterest.java
ChannelFloat32.java  Image.java            JointState.java  JoyFeedback.java       LaserScan.java  NavSatStatus.java  PointCloud.java   Range.java

After this we can regenerate rosjava_bootstrap with:

rosmake rosjava_bootstrap rosjava

Now we can use the sensor_msg/Image type. We can use as a template of the [rosjava_tutorial_pubsub package] in the same rosjava_core stack. Here you are some example code for the subscriber:

public class Listener implements NodeMain {

  public GraphName getDefaultNodeName() {
    return new GraphName("rosjava_tutorial_pubsub/listener");

  public void onStart(Node node) {
    final Log log = node.getLog();
    Subscriber<org.ros.message.sensor_msgs.image> subscriber =
        node.newSubscriber("chatter", "sensor_msgs/Image");
    subscriber.addMessageListener(new MessageListener<org.ros.message.sensor_msgs.image>() {
      public void onNewMessage(org.ros.message.sensor_msgs.Image message) {
        log.info("I heard: \"" + message.data + "\"");

  public void onShutdown(Node node) {

  public void onShutdownComplete(Node node) {

Then to launch it, we can follow the instruction explained on ROS wiki. Working over this tutorial code the only we have to do is recompiling the project and launch it with rosjava_bootstrap:

$ rosmake rosjava_tutorial_pubsub
$ rosrun rosjava_bootstrap run.py rosjava_tutorial_pubsub org.ros.tutorials.pubsub.Listener

Enjoy! I also did it as my first rosjava test 🙂

Necesitamos una sociedad y economía más justa y humanista…

2012 lo comenzamos con el anuncio de cierres de astilleros de Sevilla… En los comentarios de la noticia en el Diario de Sevilla se leen adjetivos hacia los trabajadores como: gandules, ineficientes y flojos…


Así es el libre mercado… Despiadadas empresas compiten con empresas justas, un partido de futbol donde algunas empresas pueden cometer todas las faltas que quieran. Un enfoque donde la corporación más sarnosa es premiada con la victoria. Para estas empresas el respeto a los trabajadores y el medio ambiente son una carga… Tenemos que ser conscientes que la corporación justa SIEMPRE será más cara, o visto de otro modo, más coherente y razonable con el coste que supone respetar a las personas y a nuestro planeta.

En ocasiones la empresa justa (incluso con grandes gestores e ideas) son hoy etiquetadas  con argumentos falaces económicos como “empresa poco competitiva”. Y es que todas las argumentaciones acaban en dinero…¿Hacia donde vamos?

Puede que un día nos lamentemos de la cultura industrial que estamos destruyendo, y eso no tiene precio porque la incultura nos hace esclavos de otros… Ayer comprábamos los barcos de fuera porque eran más baratos … a partir de ahora lo haremos por necesidad… Espero que no llegue el día en el que potencias extranjeras hagan negocio de nuestra incultura industrial como hoy nosotros lo hacemos con muchos países del tercer mundo.

Mientras en casa del gran gran rico..

Mientras los 200 más ricos de España ganan más dinero en uno de los peores años de la crisis, crisis que amenaza ponerlo todo patas arriba y cambiar este sistema tal y como lo conocemos… Muchos de estos ricos mueven su dinero a Luxemburgo y Holanda… el dinero es miedoso dicen…

La gran corporación privada

Y es que cada día creo menos en las grandes corporaciones privadas nacionales. Zaras, Abengoas, Telefónicas y Santanderes no buscan la mejorar esta pequeña península, sino que, como cualquier empresa beneficio a toda costa…Para mí no pueden considerarse corporaciones justas. También, cuando dicen que son grandes españolas suenan a palabras huecas y vacías dado que no existe ya un compromiso real y equilibrado con la sociedad que lo ha engendrado y donde ha florecido.

Estas empresas cada día más y más mueven los puestos de trabajo a países del tercer mundo, pagando sueldos míseros a pobres que se sienten agraciados por poder llevarse algo a la boca… Escándalos como el de Zara dan asco.También Telefónica con miles de millones de beneficios destruye empleo de un sector que un día fue público (NUESTRO) mientras crea empleos de “bajo coste” en paises menos agraciados.

Todo esto sin considerar que el día menos pensado viene un banco chino y pagará cualquier precio por adquirirla y dirigirla… Nuestra sociedad y economía que un día engendró esta industria por supuesto no se llevará ni un duro dado que los dueños (por muy españoles que sean) moverán sus beneficios a un paraíso fiscal… ¿Entonces qué nos queda?

La pequeña empresa y lo público

En mi opinión solo la pequeña industria local presenta un sólido compromiso con la misma sociedad que lo ha engendrado. Creando DE VERDAD, riqueza y cultura en su entorno, revirtiendo su propio beneficio y éxito. Por el contrario, en mi opinión, las grandes corporaciones, las grandes fortunas, con tanto poder económico y social NO PUEDEN estar fuera de un estricto control público. Esto ya lo decia Keynes incluso siendo un conservador. O públicas o semi-públicas o unas leyes de control radicales que incluyan la fuga de capitales al extranjero, etc (que por cierto sacaría el tema de discusión de una banca pública). Debemos vencer los clichés de “Lo público es anticompetitivo e ineficiente”. Incluso si así fuera, en un mercado libre donde las prácticas despreciables están justificadas en la búsqueda de beneficios

De verdad, creo en la empresa privada y creo en los emprendedores. Un país no puede funcionar sin emprendedores y arrojados… este arrojo, esfuerzo y sacrificio debe ser premiadoTodo tiene una medida y un límite: Nadie es tan bueno como para merecer lo que cobran los Botines, los Amancios y ¿por qué no? los Beckams… pero el famoseo y la salsa rosa es otra discusión más … otro reflejo de nuestra sociedad en crisis. Civilización en crisis según algunos buenos pensadores.

Basics Screens on linux

I always forge how to use the screen application in linux for ssh sesions. Here the basic stuff to work with screens:

$ screen

Exit and close:
$$ CTRL + D
$$ exit

Disconect from the screen program but leaving it opened:

Reconect to an opened screen program:
$ screen -r

From the screen, create another “tab”:

Switch selected tab:

List existing tabs: