void MultiMarker::PointCloudCorners3d(double edge_length, Pose &pose, CvPoint3D64f corners[4]) {
// Transformation from origin to current marker
CvMat *m3 = cvCreateMat(4,4,CV_64F); cvSetIdentity(m3);
pose.GetMatrix(m3);
for(size_t j = 0; j < 4; ++j)
{
// TODO: This should be exactly the same as in Marker class.
// Should we get the values from there somehow?
double X_data[4] = {0, 0, 0, 1};
if (j == 0) {
int zzzz=2;
//X_data[0] = -0.5*edge_length;
//X_data[1] = -0.5*edge_length;
} else if (j == 1) {
X_data[0] = +0.5*edge_length;
X_data[1] = -0.5*edge_length;
} else if (j == 2) {
X_data[0] = +0.5*edge_length;
X_data[1] = +0.5*edge_length;
} else if (j == 3) {
X_data[0] = -0.5*edge_length;
X_data[1] = +0.5*edge_length;
}
CvMat X = cvMat(4, 1, CV_64F, X_data);
cvMatMul(m3, &X, &X);
corners[j].x = X_data[0] / X_data[3];
corners[j].y = X_data[1] / X_data[3];
corners[j].z = X_data[2] / X_data[3];
}
cvReleaseMat(&m3);
}
bool MultiMarkerInitializer::updateMarkerPoses(vector<MarkerMeasurement> &markers, const Pose &pose) {
bool found_new = false;
for (vector<MarkerMeasurement>::iterator i = markers.begin(); i != markers.end(); ++i) {
MarkerMeasurement &marker = *i;
int id = marker.GetId();
int index = get_id_index(id);
if (index > 0 && !marker.globalPose) {
// Compute absolute marker pose.
double cam_posed[16];
double mar_posed[16];
CvMat cam_mat = cvMat(4, 4, CV_64F, cam_posed);
CvMat mar_mat = cvMat(4, 4, CV_64F, mar_posed);
pose.GetMatrix(&cam_mat);
marker.pose.GetMatrix(&mar_mat);
cvInvert(&cam_mat, &cam_mat);
cvMatMul(&cam_mat, &mar_mat, &mar_mat);
marker.pose.SetMatrix(&mar_mat);
// Put marker into point cloud
CvPoint3D64f corners[4];
PointCloudCorners3d(marker.GetMarkerEdgeLength(), marker.pose, corners);
for(size_t j = 0; j < 4; ++j) {
CvPoint3D64f p;
int p_index = pointcloud_index(id, j);
p.x = pointcloud_filtered[3*p_index+0].next(corners[j].x);
p.y = pointcloud_filtered[3*p_index+1].next(corners[j].y);
p.z = pointcloud_filtered[3*p_index+2].next(corners[j].z);
if (pointcloud_filtered[3*p_index+0].getCurrentSize() >= filter_buffer_min) {
pointcloud[p_index] = p;
marker_status[index] = 1;
}
}
marker.globalPose = 1;
found_new = true;
}
}
return found_new;
}
double MultiMarkerFiltered::_Update(MarkerIterator &begin, MarkerIterator &end,
Camera* cam, Pose& pose, IplImage* image)
{
if (_GetPose(begin, end, cam, pose, NULL) == -1) return -1;
// For every detected marker
for (MarkerIterator &i = begin.reset(); i != end; ++i)
{
const Marker* marker = *i;
int id = marker->GetId();
int index = get_id_index(id);
if (index < 0) continue;
// Initialize marker pose
if (marker_status[index] == 0)
{
double cam_posed[16];
double mar_posed[16];
CvMat cam_mat = cvMat(4, 4, CV_64F, cam_posed);
CvMat mar_mat = cvMat(4, 4, CV_64F, mar_posed);
pose.GetMatrix(&cam_mat);
marker->pose.GetMatrix(&mar_mat);
cvInvert(&cam_mat, &cam_mat);
cvMatMul(&cam_mat, &mar_mat, &mar_mat);
Pose p;
p.SetMatrix(&mar_mat);
PointCloudAverage(id, marker->GetMarkerEdgeLength(), p);
}
}
// When the pointcloud is updated we will get the new "better" pose...
return _GetPose(begin, end, cam, pose, image);
}
