openMVG----geometry

位置

Pose3 定义了[R|C]

// Define two poses and combine them
Pose3 pose1(RotationAroundX(0.02), Vec3(0,0,-2));
Pose3 pose2(RotationAroundX(0.06), Vec3(-1,0,-2));
Pose3 combinedPose = pose1 * pose2;  //应该是4*4矩阵

// Apply the pose to a 3DPoint (World to local coordinates):
const Vec3 pt = combinedPose(Vec3(2.6453,3.32,6.3));

Frustum & Frustum intersection

定义相机截锥体，根据K和Pose3：

• an infinite Frustum (4 Half Spaces) (a pyramid);
• a truncated Frustum (6 Half Spaces) (a truncated pyramid).

可以用来测试两个相机是否有共同内容。(还需要照片的大小)

// Build two truncated frustum and test their intersection
Frustum frustum1(somedata, minDepth, maxDepth);
Frustum frustum2(somedata, minDepth, maxDepth);
bool bIntersect = frustum1.intersect(frustum2);

// Build two infinite frustum and test their intersection
Frustum frustum1(somedata);
Frustum frustum2(somedata);
bool bIntersect = frustum1.intersect(frustum2);

7DoF registration between point set

点集之间的注册，有7个自由度。就是把得到的稀疏点云和真实世界对应

// Simulate two point set, apply a known transformation and estimate it back:
const int nbPoints = 10;
const Mat x1 = Mat::Random(3,nbPoints);
Mat x2 = x1;

const double scale = 2.0;
const Mat3 rot = (Eigen::AngleAxis<double>(.2, Vec3::UnitX())
  * Eigen::AngleAxis<double>(.3, Vec3::UnitY())
  * Eigen::AngleAxis<double>(.6, Vec3::UnitZ())).toRotationMatrix();
const Vec3 t(0.5,-0.3,.38);

for (int i=0; i < nbPoints; ++i)
{
  const Vec3 pt = x1.col(i);
  x2.col(i) = (scale * rot * pt + t);
}

// Compute the Similarity transform
double Sc;
Mat3 Rc;
Vec3 tc;
FindRTS(x1, x2, &Sc, &tc, &Rc);
// Optional non linear refinement of the found parameters
Refine_RTS(x1,x2,&Sc,&tc,&Rc);

std::cout << "\n"
  << "Scale " << Sc << "\n"
  << "Rot \n" << Rc << "\n"
  << "t " << tc.transpose();

std::cout << "\nGT\n"
  << "Scale " << scale << "\n"
  << "Rot \n" << rot << "\n"
  << "t " << t.transpose();

原代码

box.cpp