本文整理汇总了C++中Pose::SetRodriques方法的典型用法代码示例。如果您正苦于以下问题：C++ Pose::SetRodriques方法的具体用法？C++ Pose::SetRodriques怎么用？C++ Pose::SetRodriques使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Pose的用法示例。
在下文中一共展示了Pose::SetRodriques方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: _GetPose
double MultiMarker::_GetPose(MarkerIterator &begin, MarkerIterator &end, Camera* cam, Pose& pose, IplImage* image) {
vector<CvPoint3D64f> world_points;
vector<PointDouble> image_points;
// Reset the marker_status to 1 for all markers in point_cloud
for (size_t i=0; i<marker_status.size(); i++) {
if (marker_status[i] > 0) marker_status[i]=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;
// But only if we have corresponding points in the pointcloud
if (marker_status[index] > 0) {
for(size_t j = 0; j < marker->marker_corners.size(); ++j)
{
CvPoint3D64f Xnew = pointcloud[pointcloud_index(id, (int)j)];
world_points.push_back(Xnew);
image_points.push_back(marker->marker_corners_img.at(j));
if (image) cvCircle(image, cvPoint(int(marker->marker_corners_img[j].x), int(marker->marker_corners_img[j].y)), 3, CV_RGB(0,255,0));
}
marker_status[index] = 2; // Used for tracking
}
}
if (world_points.size() < 4) return -1;
double rod[3], tra[3];
CvMat rot_mat = cvMat(3, 1,CV_64F, rod);
CvMat tra_mat = cvMat(3, 1,CV_64F, tra);
double error=0; // TODO: Now we don't calculate any error value
cam->CalcExteriorOrientation(world_points, image_points, &rot_mat, &tra_mat);
pose.SetRodriques(&rot_mat);
pose.SetTranslation(&tra_mat);
return error;
}