A working example of condensation algorithm can be found in the Q&A of opencv and ross (same author):
http://answers.ros.org/question/55316/using-the-opencv-particle-filter-condensation/
and
http://answers.opencv.org/question/6985/syntax-for-particle-filter-in-opencv-243/