Surface makeSurfRev(const Curve &profile, unsigned steps)
{
Surface surface;
vector<Vector3f> initialVV;
if (!checkFlat(profile))
{
cerr << "surfRev profile curve must be flat on xy plane." << endl;
exit(0);
}
// TODO: Here you should build the surface. See surf.h for details.
//cerr << "\t>>> makeSurfRev called (but not implemented).\n\t>>> Returning empty surface." << endl;
for (unsigned i=0; i<profile.size(); i++){
for (unsigned j=0; j<=steps; j++){
float t = 2.0f * M_PI * float( j ) / steps;
//Matrix operations used to calulate normal
Matrix4f rotatM = Matrix4f::rotateY(t);
Matrix3f rotatMsub = rotatM.getSubmatrix3x3(0,0);
Matrix3f rotatMtrans = rotatMsub.transposed();
Matrix3f rotatN = rotatMtrans.inverse();
//Calculate surface vertex
Vector4f surfaceCalc = Vector4f(profile[i].V[0], profile[i].V[1], profile[i].V[2], 1.f);
Vector4f surfaceVecInit = rotatM*surfaceCalc;
Vector3f surfaceVec = Vector3f(surfaceVecInit[0], surfaceVecInit[1], surfaceVecInit[2]);
//Calculate surface normal
Vector3f surfaceVNInit = rotatN*profile[i].N;
//Push vectors into surface data
surface.VV.push_back(surfaceVec);
surface.VN.push_back(-1*surfaceVNInit);
}
}
//Calculate faces once all the vertices are added
for (unsigned k=0; k<surface.VV.size()-(steps+1);k++){
Tup3u firstTri; //faces uses a series of connected triangles
Tup3u secondTri;
if ((k+1)%(steps+1) != 0) //Create triangles (considering edge conditions)
{
//Triangles in counter-clockwise manner
firstTri = Tup3u(k+1, k, k+steps+1);
secondTri = Tup3u(k+1, k+1+steps, k+2+steps);
}
surface.VF.push_back(firstTri);
surface.VF.push_back(secondTri);
}
return surface;
}
int main(int argc, char** argv) {
ros::init (argc, argv, "pcl_homework");
ros::NodeHandle n;
char dir [] = "../dataset-kinect/";
char rgbPrefix []= "rgb_";
char depthPrefix []= "depth_";
char jpgSuffix []= ".jpg";
char pgmSuffix []= ".pgm";
rotated << -1, 0, 0, 0,
0, -1, 0, 0,
0, 0, -1, 0,
0, 0, 0, 1;
camera_matrix<< fx, 0.0f, cx, 0.0f, fy, cy, 0.0f, 0.0f, 1.0f;
t_mat.setIdentity();
t_mat.block<3, 3>(0, 0) = camera_matrix.inverse();
//Uncomment to show Mask Window
namedWindow("Foreground", CV_WINDOW_AUTOSIZE);
cvStartWindowThread();
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
//Uncomment to create Mask
BackgroundSubtractorMOG PMOG(20, 4, 0.9);
// Fill in the cloud data
cloud->width = 640;
cloud->height = 480;
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
for( int i = 400; i < 550; i++ ) {
char buffer1[100];
char buffer2[100];
sprintf(buffer1,"%s%s%d%s", dir, rgbPrefix, i, jpgSuffix);
sprintf(buffer2,"%s%s%d%s", dir, depthPrefix, i, pgmSuffix);
rgb_image = readJPG(buffer1);
depth_image = readPGM(buffer2);
//Calculate and show Mask
PMOG(rgb_image,fgMaskMOG);
imshow( "Foreground", fgMaskMOG );
createPC();
if(!viewer.wasStopped()){
pcl::transformPointCloud( *cloud, *cloud, rotated);
viewer.showCloud(cloud);
ros::Duration(0.25, 0).sleep();
}
}
waitKey(0);
return 0;
}
void PointWithNormalMerger::computeAccumulator() {
// Compute skin.
_indexImage.resize(480*_scale, 640*_scale);
Matrix3f _scaledCameraMatrix = _cameraMatrix;
_scaledCameraMatrix.block<2, 3>(0, 0) *= _scale;
_points.toIndexImage(_indexImage, _depthImage, _scaledCameraMatrix, Isometry3f::Identity());
// Compute sigma for each point and create image accumulator.
PixelMapper pm;
pm.setCameraMatrix(_scaledCameraMatrix);
pm.setTransform(Isometry3f::Identity());
Matrix3f inverseCameraMatrix = _scaledCameraMatrix.inverse();
float fB = (_baseLine * _scaledCameraMatrix(0, 0)); // kinect baseline * focal lenght;
CovarianceAccumulator covAcc;
_covariances.resize(480*_scale, 640*_scale);
_covariances.fill(covAcc);
for(size_t i = 0; i < _points.size(); i++ ) {
PointWithNormal &p = _points[i];
Vector2i coord = pm.imageCoords(pm.projectPoint(p.point()));
if(coord[0] < 0 || coord[0] >= _depthImage.cols() ||
coord[1] < 0 || coord[1] >= _depthImage.rows())
continue;
int index = _indexImage(coord[1], coord[0]);
float skinZ = _depthImage(coord[1], coord[0]);
Vector3f normal = p.normal();
Vector3f skinNormal = _points[index].normal();
float z = p[2];
if(abs(z - skinZ) > 0.05)
continue;
if(acosf(skinNormal.dot(normal)) > M_PI/4.0f)
continue;
float zVariation = (_alpha*z*z)/(fB+z*_alpha);
zVariation *= zVariation;
Diagonal3f imageCovariance(1.0f, 1.0f, zVariation);
Matrix3f covarianceJacobian;
covarianceJacobian <<
z, 0, (float)coord[0],
0, z, (float)coord[1],
0, 0, 1;
covarianceJacobian = inverseCameraMatrix*covarianceJacobian;
Matrix3f worldCovariance = covarianceJacobian * imageCovariance * covarianceJacobian.transpose();
_covariances(coord[1], coord[0])._omegaAcc += worldCovariance.inverse();
_covariances(coord[1], coord[0])._pointsAcc += worldCovariance.inverse()*p.point();
_covariances(coord[1], coord[0])._used = true;
}
}
int main()
{
Matrix3f A;
A << 1, 2, 1,
2, 1, 0,
-1, 1, 2;
cout << "Here is the matrix A:\n" << A << endl;
cout << "The determinant of A is " << A.determinant() << endl;
cout << "The inverse of A is:\n" << A.inverse() << endl;
}
int main(int, char**)
{
cout.precision(3);
Matrix3f A;
Vector3f b;
A << 1,2,3, 4,5,6, 7,8,10;
b << 3, 3, 4;
Vector3f x = A.inverse() * b;
cout << "The solution is:" << endl << x << endl;
return 0;
}
void testInversion()
{
Matrix3f rab = ma * mb;
Matrix3f rabInv = mab.inverse();
/*
std::cout << "ma * mb=\n" << rab << std::endl;
std::cout << "inv(ma*mb) computed=\n" << rabInv << std::endl;
std::cout << "inv(ma*mb) expected=\n" << mabInv << std::endl;
*/
CPPUNIT_ASSERT(rabInv == mabInv);
}
void Lu::Initialize(MatrixXf x3d_h,MatrixXf x2d_h, Matrix3f A) {
x2d=x2d_h.transpose();
// std::cout<<"x2dtrasn"<<x2d<<std::endl;
x3d=x3d_h.transpose();
//std::cout<<"x3dtrasn"<<x3d<<std::endl;
x2dn=A.inverse ()*x2d;
//std::cout<<"x2dn"<<x2dn<<std::endl;
K=A;
//std::cout<<"Intialize points "<<std::endl;
}
void computeHomographyResidue(MatrixXf pts1, MatrixXf pts2, const Matrix3f &H, MatrixXf &residue){
// cross residue
filterPointAtInfinity(pts1, pts2);
residue.resize(pts1.rows(), 1);
MatrixXf Hx1 = (H*pts1.transpose()).transpose();
MatrixXf invHx2 = (H.inverse()*pts2.transpose()).transpose();
noHomogeneous(Hx1);
noHomogeneous(invHx2);
noHomogeneous(pts1);
noHomogeneous(pts2);
MatrixXf diffHx1pts2 = Hx1 - pts2;
MatrixXf diffinvHx2pts1 = invHx2 - pts1;
residue = diffHx1pts2.rowwise().squaredNorm() + diffinvHx2pts1.rowwise().squaredNorm();
}
Vector3f computePlaneIntersection( const Plane& plane0, const Plane& plane1, const Plane& plane2 )
{
// TODO: Paralllel planes, whew !
Matrix3f linSolve;
linSolve.set_row( 0, plane0.getNormal() );
linSolve.set_row( 1, plane1.getNormal() );
linSolve.set_row( 2, plane2.getNormal() );
Matrix3f ilinSolve;
linSolve.inverse( ilinSolve );
Vector3f bSolve( -plane0.getd(),
-plane1.getd(),
-plane2.getd() );
return ilinSolve * bSolve;
}
void Physics_testTest::testCase1()
{
Matrix3f matrix(Matrix3f::kZero);
Matrix3f identity(Matrix3f::kIdentity);
Matrix3f sum(identity + identity);
Matrix3f sum2 = identity * 2.0f;
QVERIFY2(sum2 - sum == Matrix3f::kZero, "Sum" );
QVERIFY2(sum2.det() == 8.0f, "Det");
Matrix3f inverse = sum2.inverse();
QVERIFY2(inverse.det() - (1.0f/8.0f) < 0.01f, "Inverse");
QVERIFY2(inverse*sum2 == Matrix3f::kIdentity, "Inverse2");
Matrix3f rotX = Matrix3f::RotationX(0.1);
Matrix3f rotY = Matrix3f::RotationY(0.5);
Matrix3f rotZ = Matrix3f::RotationZ(2.0);
Matrix3f total = rotX * rotY * rotZ;
QVERIFY2(fabs(((total * total.inverse()) - Matrix3f::kIdentity).det()) < 0.00000001f, "Complex");
Vector3f y_up = Vector3f(0.0f, 1.0f, 0.0f);
Vector3f x_right = Vector3f(1.0f, 0.0f, 0.0f);
Vector3f imageY = rotX * y_up;
QVERIFY2(fabs(imageY.dot(y_up) - cosf(0.1f)) < 0.00001, "Rotation");
Matrix3f rot_axis = Matrix3f::FromAxisAngle(y_up, 0.5);
Vector3f imageX = rot_axis * y_up;
QVERIFY2(fabs(imageX.dot(y_up) - y_up.lengthSquared()) < 0.00001, "Axis Angle");
imageX = rot_axis * x_right;
QVERIFY2(fabs(imageX.dot(x_right) - cosf(0.5)) < 0.00001, "Axis Angle");
//Matrix3f identity = Matrix3f::kIdentity;
QVERIFY2(true, "Failure");
}
Matrix3f A; Vector3f b; A << 1,2,3, 4,5,6, 7,8,10; b << 3, 3, 4; Vector3f x = A.inverse() * b; cout << "The solution is:" << endl << x << endl;
QImage CVlib::generateImage(QImage imageBase, Matrix3f h, QVector<Vector3f> *renderArea)
{
QVector<Vector3f> areaTemp;
if(renderArea==0){
areaTemp.push_back(Vector3f(0,0,1));
areaTemp.push_back(Vector3f(0,imageBase.height(),1));
areaTemp.push_back(Vector3f(imageBase.width(),imageBase.height(),1));
areaTemp.push_back(Vector3f(imageBase.width(),0,1));
}else{
areaTemp = *renderArea;
}
bounds limits = CVlib::getHomographyBounds(areaTemp, h);
QSize size;
float ratio;
float tamanhoBase =500;
// if(imageBase.width()>imageBase.height()){
// tamanhoBase = imageBase.width();
// }else{
// tamanhoBase = imageBase.height();
// }
if(limits.dx < limits.dy){
qDebug() << "dx";
ratio = limits.dx/ limits.dy;
size = QSize(tamanhoBase* ratio, tamanhoBase);
}else{
qDebug() << "dy";
ratio = limits.dy/ limits.dx;
size = QSize(tamanhoBase , tamanhoBase*ratio);
//size = QSize(imageBase.width() * ratio, imageBase.width());
}
//ratio = limits.dx/ limits.dy;
//size = QSize(300 * ratio, 300);
QImage imageResult = QImage(size.width(), size.height(), QImage::Format_ARGB32);
//TODO testar imagem horizontal e vertical
//TODO implementar outros trabalhos
//Todo entrada de paramentro baseado no tamanho de saida da imagem
float factor = 1;
float stepX = limits.dx / size.width() / factor;
float stepY = limits.dy / size.height() /factor;
Matrix3f hi = h.inverse().eval();
for(int j = 0; j<imageResult.height(); j++){
for(int i = 0; i<imageResult.width(); i++){
MatrixXf p(3,1);
p << (limits.left + stepX * i) , (limits.top + stepY * j), 1;
MatrixXf r(3,1);
r= CVlib::homography(p, hi);
QPoint p0(r(0),r(1));
QColor color(0, 0, 0);
MatrixXf finalColor(3,1);
finalColor << 0, 0, 0;
float ratio = 1;
if((p0.x() >=0 && p0.x() < imageBase.width())&&(p0.y()>=0 && p0.y()<imageBase.height())){
ratio = 0;
for(int offSetY = -1; offSetY <= 1; offSetY++){
for(int offSetX = -1; offSetX <= 1; offSetX++){
QPoint ptemp(p0.x() + offSetX, p0.y() + offSetY);
float ratioX = r(0)-ptemp.x();
float ratioY = r(1)-ptemp.y();
float dist = std::abs(1 - std::sqrt(std::pow(ratioX, 2) + std::pow(ratioY,2)));
ratio += dist;
if((ptemp.x() >=0 && ptemp.x() < imageBase.width())&&(ptemp.y()>=0 && ptemp.y()<imageBase.height())){
QColor ctemp = imageBase.pixel(ptemp.x(), ptemp.y());
MatrixXf cmTemp(3,1);
cmTemp << ctemp.redF(), ctemp.greenF(), ctemp.blueF();
finalColor += cmTemp * dist;
}
}
}
}
finalColor = finalColor/ratio;
color.setRgb(finalColor(0) * 255, finalColor(1) * 255, finalColor(2) * 255);
imageResult.setPixel(QPoint(i, j), color.rgb());
}
}
return imageResult;
}
int Lu::objpose(MatrixXf P, MatrixXf Qp)
{
int n = P.cols();
int i;
// move the origin to the center of P
// std::cout<<"P\n"<<P<<std::endl;
Vector3f pbar=Vector3f::Zero();
// std::cout<<"pbar\n"<<pbar<<std::endl;
for (i = 0; i<n;i++){
// std::cout<<"Pcol\n"<<P.col(i)<<std::endl;
pbar=pbar+P.col(i);
}
//std::cout<<"pbar\n"<<pbar;
pbar= pbar/n;
// std::cout<<"move the origin to the center of P"<<std::endl;
//std::cout<<"P\n"<<P;
//std::cout<<"pbar\n"<<pbar<<std::endl;
for (i = 0; i<n;i++){
P.col(i) = P.col(i)-pbar;
}
//std::cout<<"P\n"<<P<<std::endl;
MatrixXf Q;
Q=MatrixXf::Zero(3,n);
for (i = 0 ;i<n;i++){
Q(0,i)=Qp(0,i);
Q(1,i)=Qp(1,i);
Q(2,i)=1;
}
//std::cout<<"Q\n"<<Q<<std::endl;
//create matrix F with 0's
//Matrix3f maux;
//for (i=0;i<n;i++ ){
// F.push_back(Matrix3f::Zero(3,3));
//}
RowVector3f V;
V= RowVector3f::Zero();
std::vector<Matrix3f > F;
Matrix3f F_aux;
for (i = 0 ;i<n;i++){
V[0] = Q(0,i)/Q(2,i);
V[1] = Q(1,i)/Q(2,i);
V[2] = Q(2,i)/Q(2,i);
//std::cout<<"V\n"<<V<<std::endl;
Matrix3f F_aux;
F_aux=(V.transpose()*V)/V.dot(V);
//std::cout<<"F_aux\n"<<F_aux<<std::endl;
// F_aux = (V*V.transpose())/(V.transpose()*V);
F.push_back(F_aux);
}
//compute the matrix factor required to compute t
Matrix3f F_sum=Matrix3f::Zero(3,3);
for (i = 0 ;i< n;i++){
F_sum=F_sum + F[i];
}
//std::cout<<"F_sum\n"<<F_sum<<std::endl;
Matrix3f tFactor = (Matrix3f::Identity()-F_sum/n);
tFactor=tFactor.inverse()/n;
//std::cout<<"tfactor\n"<<tFactor<<std::endl;
MatrixXf Ri_;
MatrixXf ti_;
MatrixXf Qi_;
double old_err;
// compute initial pose estimate
abskernel(P, Q, F, tFactor);
//Lu::abskernel(MatrixXf P, MatrixXf Q, std::vector<Matrix3f > F, MatrixXf G)
Ri_=GetRi();
ti_=Getti();
Qi_=GetQout();
int it = 1;
old_err=Geterr();
/*std::cout<<"Ri_\n"<<Ri_<<std::endl;
std::cout<<"ti_\n"<<ti_<<std::endl;
std::cout<<"Qi_\n"<<Qi_<<std::endl;
std::cout<<"old_err\n"<<old_err<<std::endl;*/
//std::cout<<"old_err\n"<<old_err<<std::endl;
// compute next pose estimate
abskernel(P, Qi_, F, tFactor);
Ri_=GetRi();
ti_=Getti();
Qi_=GetQout();
double new_err=Geterr();
it = it + 1;
/*std::cout<<"Ri_2\n"<<Ri_<<std::endl;
std::cout<<"ti_2\n"<<ti_<<std::endl;
std::cout<<"Qi_2\n"<<Qi_<<std::endl;
std::cout<<"new_err\n"<<new_err<<std::endl;*/
//std::cout<<"new_err\n"<<new_err<<std::endl;
double val=(old_err-new_err)/old_err;
val=sqrt(val*val);
/* std::cout<<"val\n"<<val<<std::endl;
std::cout<<"TOL\n"<<TOL<<std::endl;
std::cout<<"EPSILON\n"<<EPSILON<<std::endl;
std::cout<<"MAX_ITER\n"<<MAX_ITER<<std::endl;*/
while ((val > TOL) && (new_err > EPSILON) && (it<MAX_ITER)){
old_err = new_err;
//std::cout<<"old_err\n"<<old_err<<std::endl;
// compute the optimal estimate of R
abskernel(P, Qi_, F, tFactor);
Ri_=GetRi();
ti_=Getti();
Qi_=GetQout();
new_err=Geterr();
it = it + 1;
}
R = Ri;
t = ti_;
obj_err = sqrt(new_err/n);
//double img_err = sqrt(img_err/n);
t = t - Ri*pbar;
return 0;
}
Surface makeGenCyl(const Curve &profile, const Curve &sweep )
{
Surface surface;
if (!checkFlat(profile))
{
cerr << "genCyl profile curve must be flat on xy plane." << endl;
exit(0);
}
// TODO: Here you should build the surface. See surf.h for details.
//cerr << "\t>>> makeGenCyl called (but not implemented).\n\t>>> Returning empty surface." <<endl;
for (unsigned i=0; i<profile.size(); i++){
for (unsigned j=0; j<sweep.size(); j++){
//Matrix from sweep
Matrix4f coordM(sweep[j].N[0], sweep[j].B[0], sweep[j].T[0], sweep[j].V[0],
sweep[j].N[1], sweep[j].B[1], sweep[j].T[1], sweep[j].V[1],
sweep[j].N[2], sweep[j].B[2], sweep[j].T[2], sweep[j].V[2],
0.f, 0.f, 0.f, 1.f);
//Matrix operations to get normal
Matrix3f rotatMsub = coordM.getSubmatrix3x3(0,0);
Matrix3f rotatMtrans = rotatMsub.transposed();
Matrix3f rotatN = rotatMtrans.inverse();
//Calculate surface vertex
Vector4f surfaceCalc = Vector4f(profile[i].V[0], profile[i].V[1], profile[i].V[2], 1.f);
Vector4f surfaceVecInit = coordM*surfaceCalc;
Vector3f surfaceVec = Vector3f(surfaceVecInit[0], surfaceVecInit[1], surfaceVecInit[2]);
//Calculate surface normal
Vector3f surfaceVNInit = rotatN*profile[i].N;
//Push vectors into surface data
surface.VV.push_back(surfaceVec);
surface.VN.push_back(-1*surfaceVNInit);
}
}
//Calculate faces once all the vertices are added
for (unsigned k=0; k<surface.VV.size()-(sweep.size());k++){
Tup3u firstTri; //faces uses a series of connected triangles
Tup3u secondTri;
if ((k+1)%(sweep.size()) != 0) //Create triangles (considering edge conditions)
{
//Triangles in counter-clockwise manner
firstTri = Tup3u(k+1, k, k+sweep.size());
secondTri = Tup3u(k+1, k+sweep.size(), k+1+sweep.size());
}
surface.VF.push_back(firstTri);
surface.VF.push_back(secondTri);
}
return surface;
}
Vector4f pointAlignerIteration(Vector3f x, MatrixXf Z){
Matrix3f H;
Vector3f b;
Vector4f xNew_chi;
H << 0,0,0,
0,0,0,
0,0,0;
b << 0,
0,
0;
float chi = 0;
Matrix3f X = v2tRad(x);
//cout << "matrix X from inside the point align iteration: \n" << X << endl;
Matrix2f Omega;
Omega << 1, 0,
0, 1;
Vector2f e;
Jacobian jac;
for(int i=0; i < Z.cols(); i++){
e = computeError(i, X, Z);
jac = computeJacobian(i, X, Z);
//cout << "jacobian: \n" << jac << endl;
//getc(stdin);
H += jac.transpose()*Omega*jac;
b += jac.transpose()*Omega*e;
chi += e.transpose()*Omega*e;
}
Vector3f dX;
dX = -H.inverse()*b;
//cout << "H inverse: \n" << H.inverse() << endl;
//getc(stdin);
for(int i=0; i<3; i++){
xNew_chi(i, 0) = x(i, 0) + dX(i, 0);
}
xNew_chi(2, 0) = atan2(sin(xNew_chi(2,0)), cos(xNew_chi(2, 0)));
xNew_chi(3, 0) = chi;
return xNew_chi;
}
