void MiniGL::setViewport (float pfovy, float pznear, float pzfar, const Vector3f &peyepoint, const Vector3f &plookat)
{
fovy = pfovy;
znear = pznear;
zfar = pzfar;
glLoadIdentity ();
gluLookAt (peyepoint [0], peyepoint [1], peyepoint [2], plookat[0], plookat[1], plookat[2], 0, 1, 0);
Matrix4f transformation;
float *lookAtMatrix = transformation.data();
glGetFloatv (GL_MODELVIEW_MATRIX, &lookAtMatrix[0]);
Matrix3f rot;
Vector3f scale;
transformation.transposeInPlace();
rot.row(0) = Vector3f (transformation(0,0), transformation(0,1), transformation(0,2));
rot.row(1) = Vector3f (transformation(1,0), transformation(1,1), transformation(1,2));
rot.row(2) = Vector3f (transformation(2,0), transformation(2,1), transformation(2,2));
scale[0] = rot.col(0).norm();
scale[1] = rot.col(1).norm();
scale[2] = rot.col(2).norm();
m_translation = Vector3f (transformation(3,0), transformation(3,1), transformation(3,2));
rot.col(0) = 1.0f/scale[0] * rot.col(0);
rot.col(1) = 1.0f/scale[1] * rot.col(1);
rot.col(2) = 1.0f/scale[2] * rot.col(2);
m_zoom = scale[0];
m_rotation = Quaternionf(rot);
glLoadIdentity ();
}
void Camera::SetView(const Vector3f &position, const Vector3f &target,
const Vector3f &up) {
Matrix3f rotation;
rotation.row(1) = up.normalized();
rotation.row(2) = (position - target).normalized();
rotation.row(0) = rotation.row(1).cross(rotation.row(2));
view_.topLeftCorner<3, 3>() = rotation;
view_.topRightCorner<3, 1>() = -rotation * position;
view_(3, 3) = 1.0;
matrix_dirty_ = true;
}
int main(int, char**)
{
cout.precision(3);
Matrix3f m;
m.row(0) << 1, 2, 3;
m.block(1,0,2,2) << 4, 5, 7, 8;
m.col(2).tail(2) << 6, 9;
std::cout << m;
return 0;
}
Eigen::Matrix4f ConsistencyTest::initPose( std::map<unsigned, unsigned> &matched_planes )
{
//Calculate rotation
Matrix3f normalCovariances = Matrix3f::Zero();
for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
normalCovariances += PBMTarget.vPlanes[it->second].v3normal * PBMSource.vPlanes[it->first].v3normal.transpose();
JacobiSVD<MatrixXf> svd(normalCovariances, ComputeThinU | ComputeThinV);
Matrix3f Rotation = svd.matrixU() * svd.matrixV().transpose();
if(Rotation.determinant() < 0)
Rotation.row(2) *= -1;
// Rotation = -Rotation;
// Calculate translation
Vector3f translation;
Vector3f center_data = Vector3f::Zero(), center_model = Vector3f::Zero();
Vector3f centerFull_data = Vector3f::Zero(), centerFull_model = Vector3f::Zero();
unsigned numFull = 0, numNonStruct = 0;
for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
{
if(PBMSource.vPlanes[it->first].bFromStructure) // The certainty in center of structural planes is too low
continue;
++numNonStruct;
center_data += PBMSource.vPlanes[it->first].v3center;
center_model += PBMTarget.vPlanes[it->second].v3center;
if(PBMSource.vPlanes[it->first].bFullExtent)
{
centerFull_data += PBMSource.vPlanes[it->first].v3center;
centerFull_model += PBMTarget.vPlanes[it->second].v3center;
++numFull;
}
}
if(numFull > 0)
{
translation = (centerFull_model - Rotation * centerFull_data) / numFull;
// translation = (centerFull_data - Rotation * centerFull_model) / numFull;
}
else
{
translation = (center_model - Rotation * center_data) / numNonStruct;
// translation = (center_data - Rotation * center_model) / numNonStruct;
}
// Form SE3 transformation matrix. This matrix maps the model into the current data reference frame
Eigen::Matrix4f rigidTransf;
rigidTransf.block(0,0,3,3) = Rotation;
rigidTransf.block(0,3,3,1) = translation;
rigidTransf.row(3) << 0,0,0,1;
return rigidTransf;
}
void visualizerShowCamera(const Matrix3f& R, const Vector3f& _t, float r, float g, float b, double s = 0.01 /*downscale factor*/, const std::string& name = "") {
std::string name_ = name,line_name = name + "line";
if (name.length() <= 0) {
stringstream ss; ss<<"camera"<<iCamCounter++;
name_ = ss.str();
ss << "line";
line_name = ss.str();
}
Vector3f t = -R.transpose() * _t;
Vector3f vright = R.row(0).normalized() * s;
Vector3f vup = -R.row(1).normalized() * s;
Vector3f vforward = R.row(2).normalized() * s;
Vector3f rgb(r,g,b);
pcl::PointCloud<pcl::PointXYZRGB> mesh_cld;
mesh_cld.push_back(Eigen2PointXYZRGB(t,rgb));
mesh_cld.push_back(Eigen2PointXYZRGB(t + vforward + vright/2.0 + vup/2.0,rgb));
mesh_cld.push_back(Eigen2PointXYZRGB(t + vforward + vright/2.0 - vup/2.0,rgb));
mesh_cld.push_back(Eigen2PointXYZRGB(t + vforward - vright/2.0 + vup/2.0,rgb));
mesh_cld.push_back(Eigen2PointXYZRGB(t + vforward - vright/2.0 - vup/2.0,rgb));
//TODO Mutex acquire
pcl::PolygonMesh pm;
pm.polygons.resize(6);
for(int i=0;i<6;i++)
for(int _v=0;_v<3;_v++)
pm.polygons[i].vertices.push_back(ipolygon[i*3 + _v]);
pcl::toROSMsg(mesh_cld,pm.cloud);
bShowCam = true;
cam_meshes.push_back(std::make_pair(name_,pm));
//TODO mutex release
linesToShow.push_back(std::make_pair(line_name,
AsVector(Eigen2Eigen(t,rgb),Eigen2Eigen(t + vforward*3.0,rgb))
));
}
int main()
{
Matrix3f m;
m << 1, 2, 3,
4, 5, 6,
7, 8, 9;
std::cout << m << std::endl;
RowVectorXd vec1(3);
vec1 << 1, 2, 3;
std::cout << "vec1 = " << vec1 << std::endl;
RowVectorXd vec2(4);
vec2 << 1, 4, 9, 16;;
std::cout << "vec2 = " << vec2 << std::endl;
RowVectorXd joined(7);
joined << vec1, vec2;
std::cout << "joined = " << joined << std::endl;
MatrixXf matA(2, 2);
matA << 1, 2, 3, 4;
MatrixXf matB(4, 4);
matB << matA, matA/10, matA/10, matA;
std::cout << matB << std::endl;
{
Matrix3f m;
m.row(0) << 1, 2, 3;
m.block(1,0,2,2) << 4, 5, 7, 8;
m.col(2).tail(2) << 6, 9;
std::cout << m << std::endl;
}
return 0;
}
