- fixed uninitialized memory access and memory leaks
- extracted several code blocks to separate functions
- updated part of algorithm to use cv::Mat instead of CvMat and IplImage