最近研究相机,看一些相机位姿和相机外参,但总是将世界坐标系和相机坐标系进行记错,,也不理解什么是相机位姿和相机外参的关系,在实际应用中发现很多问题。所以这几天在实际应用中,重新对于相机坐标和世界坐标系的讨论和实践,然后整理了一下。
看了一下《视觉SLAM》,
画出相机在世界坐标系的位置和它的朝向代码(z轴就是朝向)它保存为obj文件如下:
void SaveCamPoseIntoOBJ(std::vector<Eigen::Matrix4f>& cams_pose, const std::string& path)
{
std::ofstream fh(path);
int step_size = 1000;
float step = 0.05;
for (int i = 0; i < cams_pose.size(); ++i)
{
Eigen::Matrix3f R_wc = cams_pose[i].topLeftCorner(3, 3);
Eigen::Vector3f T_wc = cams_pose[i].topRightCorner(3, 1);
--------------------三根轴------------------------//
//x轴
Eigen::Vector3f local_x = R_wc.col(0);
//y轴
Eigen::Vector3f local_y = R_wc.col(1);
//z轴
Eigen::Vector3f local_z = R_wc.col(2);
std::vector<Eigen::Vector3f> cam_x_local_pnts;
std::vector<Eigen::Vector3f> cam_y_local_pnts;
std::vector<Eigen::Vector3f> cam_z_local_pnts;
for (int i = 0; i < step_size; ++i)
{
//x轴
float x_x = T_wc(0) + i * step * local_x(0);
float x_y = T_wc(1) + i * step * local_x(1);
float x_z = T_wc(2) + i * step * local_x(2);
Eigen::Vector3f tmp_x(x_x, x_y, x_z);
cam_x_local_pnts.emplace_back(tmp_x);
//y轴
float y_x = T_wc(0) + i * step * local_y(0);
float y_y = T_wc(1) + i * step * local_y(1);
float y_z = T_wc(2) + i * step * local_y(2);
Eigen::Vector3f tmp_y(y_x, y_y, y_z);
cam_y_local_pnts.emplace_back(tmp_y);
//z轴
float z_x = T_wc(0) + i * step * local_z(0);
float z_y = T_wc(1) + i * step * local_z(1);
float z_z = T_wc(2) + i * step * local_z(2);
Eigen::Vector3f tmp_z(z_x, z_y, z_z);
cam_z_local_pnts.emplace_back(tmp_z);
}
for (int i = 0; i < step_size; ++i)
{
fh << "v " << cam_x_local_pnts[i](0) << " " << cam_x_local_pnts[i](1) << " " << cam_x_local_pnts[i](2) <<
" " << 255 << " " << 0 << " " << 0 << std::endl;
fh << "v " << cam_y_local_pnts[i](0) << " " << cam_y_local_pnts[i](1) << " " << cam_y_local_pnts[i](2) <<
" " << 0 << " " << 255 << " " << 0 << std::endl;
fh << "v " << cam_z_local_pnts[i](0) << " " << cam_z_local_pnts[i](1) << " " << cam_z_local_pnts[i](2) <<
" " << 0 << " " << 0 << " " << 255 << std::endl;
}
}
fh.close();
}
上面的代码使用的是点云画出相机的三根轴的方向,可能不好用,可以使用另一种线段方式表达相机的三根轴。同样保存为obj文件,它和上面的效果一样。
void SaveAllCamsPoseIntoObj(const std::string& path, std::vector<Eigen::Matrix4d> cmps, float aixlen)
{
for(std::size_t i = 0; i < cmps.size(); ++i)
{
//x,y,z aix
Eigen::Vector3d lx = cmps[i].block<3,1>(0,0);
Eigen::Vector3d ly = cmps[i].block<3,1>(0,1);
Eigen::Vector3d lz = cmps[i].block<3,1>(0,2);
Eigen::Vector3d t = cmps[i].block<3,1>(0,3);
lx.normalize();
ly.normalize();
lz.normalize();
Eigen::Vector3d ex = t+aixlen*lx;
Eigen::Vector3d ey = t+aixlen*ly;
Eigen::Vector3d ez = t+aixlen*lz;
fh << "v " << t[0] << " " << t[1] << " " << t[2]
<< " " << 255 << " " << 255 << " " << 255 << std::endl;
fh << "v " << ex[0] << " " << ex[1] << " " << ex[2]
<< " " << 255 << " " << 0 << " " << 0 << std::endl;
fh << "v " << ey[0] << " " << ey[1] << " " << ey[2]
<< " " << 0 << " " << 255 << " " << 0 << std::endl;
fh << "v " << ez[0] << " " << ez[1] << " " << ez[2]
<< " " << 0 << " " << 0 << " " << 255 << std::endl;
}
for(std::size_t i = 0 ; i < cmps.size(); ++i)
{
fh << "l " << 4*i+1 << " " << 4*i+2 << std::endl;
fh << "l " << 4*i+1 << " " << 4*i+3 << std::endl;
fh << "l " << 4*i+1 << " " << 4*i+4 << std::endl;
}
fh.close();
}
保存为obj文件,使用meshlab文件打开即可
得到的如下:
2,下面是相机参数定义:
3:相机外参和相机位姿的关系:
4:实践中的投影(从三维点投影到二维图像上)应用和画出相机(调试):