void ofxIcp::compute(const vector<ofPoint> & initial, const vector<ofPoint> & target, vector<ofPoint> & output, ofMatrix4x4 & transformation, double & error, int & iterations){
vector<cv::Point3d> initial_cv;
vector<cv::Point3d> target_cv;
toCV(initial, initial_cv);
toCV(target, target_cv);
cv::Mat transformation_matrix;
vector<cv::Point3d> output_cv;
compute(initial_cv, target_cv, output_cv, transformation_matrix, error, iterations);
toOF(output_cv, output);
transformation.set(transformation_matrix.ptr<double>());
}
cv::Mat toCV(const Array& arr) {
// This for OpenCV2:
// size_t steps[ALLO_ARRAY_MAX_DIMS];
// int sizes[ALLO_ARRAY_MAX_DIMS];
// for (int i=0; i<arr.header.dimcount; i++) {
// steps[i] = arr.header.stride[i];
// sizes[i] = arr.header.dim[i];
// }
// return CvMat(
// arr.header.dimcount,
// sizes,
// toCV(arr.header.type, arr.header.components),
// arr.data.ptr,
// steps
// );
int type = toCV(arr.header.type, arr.header.components);
return cv::Mat(arr.dim(1), arr.dim(0), type, (uchar *)arr.data.ptr, arr.stride(1));
}
