void ofxARToolkitPlus::getTranslationAndOrientation(int markerIndex, ofVec3f &translation, ofMatrix4x4 &orientation) {
ARToolKitPlus::ARMarkerInfo marker = tracker->getDetectedMarker(markerIndex);
getTransMat( &marker, c, m34 );
// Translation
translation.set(m34[0][3], m34[1][3], m34[2][3]);
// Orientation
orientation.set(m34[0][0], m34[0][1], m34[0][2], 0,
m34[1][0], m34[1][1], m34[1][2], 0,
m34[2][0], m34[2][1], m34[2][2], 0,
0, 0, 0, 1);
}
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>());
}
void ofxARToolkitPlus::getMultiMarkerTranslationAndOrientation(ofVec3f &translation, ofMatrix4x4 &orientation) {
const ARToolKitPlus::ARMultiMarkerInfoT *multiMarkerConst = tracker->getMultiMarkerConfig();
if(multiMarkerConst != NULL) {
// Create a copy of the ARMultiMarkerInfoT struct
ARToolKitPlus::ARMultiMarkerInfoT mm;
size_t mmSize = sizeof(ARToolKitPlus::ARMultiMarkerInfoT);
memcpy(&mm, multiMarkerConst, mmSize);
// Copy and pass in the markers
int numberOfMarkers = tracker->getNumDetectedMarkers();
#ifdef TARGET_WIN32
ARToolKitPlus::ARMarkerInfo *marker = new ARToolKitPlus::ARMarkerInfo[numberOfMarkers];
#else
ARToolKitPlus::ARMarkerInfo marker[numberOfMarkers];
#endif
for (int i=0; i<numberOfMarkers; i++) {
marker[i] = tracker->getDetectedMarker(i);
}
float result = tracker->rppMultiGetTransMat(marker, numberOfMarkers, &mm);
// Check for error - yes this does occur
if(result < 0 || result >= INT_MAX) {
tracker->arMultiGetTransMat(marker, numberOfMarkers, &mm);
ofLog(OF_LOG_VERBOSE, "RPP failed on multimarker");
}
// Translation
translation.set(mm.trans[0][3], mm.trans[1][3], mm.trans[2][3]);
// Orientation
orientation.set(mm.trans[0][0], mm.trans[0][1], mm.trans[0][2], 0,
mm.trans[1][0], mm.trans[1][1], mm.trans[1][2], 0,
mm.trans[2][0], mm.trans[2][1], mm.trans[2][2], 0,
0, 0, 0, 1);
#ifdef TARGET_WIN32
free(marker);
#endif
} else {
ofLog(OF_LOG_VERBOSE, "MultiMarkerConfig file NULL");
}
}
