Consider that two points p0 and p1 are at euclidian distance d1 and the Lab color value of given two pixels/voxels are (L0,a0,b0) and (L1,a1,b1). Now if i change the position of a rigid object which contains given two points than shouldn't it be possible to detect those points at the same distance(mm) and with the same Lab color value. I am using pcl to do computer vision task of 3d object recognition. Here when i try to find the two points previously detected at distance d1 and its colors and not being detected if i change the position and oriantation of the object from which given two points, voxels here were analyzed. The object is not oriented that the same two points is not visible to the kinect camera i'm using.
iv=1; int p0, p1, p2, p0obj, p1obj, p2obj;
for(p0=0;p0<a.size() && ros::ok() && iv==1;p0++) {
for(p1=0;p1<a.size() && ros::ok() && iv==1;p1++) {
int d1 = sqrt(pow(a.at(p1)-a.at(p0),2)+pow(b.at(p1)-b.at(p0),2)+pow(c.at(p1)-c.at(p0),2))*1000;
if(d1==20) {
for(p2=0;p2<a.size() && ros::ok() && iv==1;p2++) {
int d2 = sqrt(pow(a.at(p2)-a.at(p0),2)+pow(b.at(p2)-b.at(p0),2)+pow(c.at(p2)-c.at(p0),2))*1000;
int d1d = sqrt(pow(a.at(p2)-a.at(p1),2)+pow(b.at(p2)-b.at(p1),2)+pow(c.at(p2)-c.at(p1),2))*1000;
if(d2==20 && d1d==20) {
float a1 = a.at(p1)-a.at(p0); float b1 = b.at(p1)-b.at(p0); float c1 = c.at(p1)-c.at(p0);
float a2 = a.at(p2)-a.at(p0); float b2 = b.at(p2)-b.at(p0); float c2 = c.at(p2)-c.at(p0);
float a3r = b1*c2-b2*c1; float b3r = a2*c1-a1*c2; float c3r = a1*b2-a2*b1;
float a3, b3, c3;
if(c3r>0) {
a3 = a3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
b3 = b3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
c3 = -c3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
}
else {
a3 = a3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
b3 = b3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
c3 = c3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
}
float x3 = (a.at(p0)+a.at(p1)+a.at(p2)/3)+0.02*a3;
float y3 = (b.at(p0)+b.at(p1)+b.at(p2)/3)+0.02*b3;
float z3 = (c.at(p0)+c.at(p2)+c.at(p2)/3)+0.02*c3;
for(int p4=0;p4<a.size() && ros::ok() && iv==1;p4++) {
int d0r = sqrt(pow(a.at(p4)-a.at(p0),2)+pow(b.at(p4)-b.at(p0),2)+pow(c.at(p4)-c.at(p0),2))*1000;
int d1r = sqrt(pow(a.at(p4)-a.at(p1),2)+pow(b.at(p4)-b.at(p1),2)+pow(c.at(p4)-c.at(p1),2))*1000;
int d2r = sqrt(pow(a.at(p4)-a.at(p2),2)+pow(b.at(p4)-b.at(p2),2)+pow(c.at(p4)-c.at(p2),2))*1000;
int d4r = sqrt(pow(x3-a.at(p1),2)+pow(y3-b.at(p1),2)+pow(z3-c.at(p1),2));
if(d0r>0 && d1r>0 && d2r>0 && d4r>0 && d0r<=70) {
cout<<p0<<endl;
d[0]=p0; d[1]=p1; d[2]=p2; d[3]=d0r; d[4]=d1r; d[5]=d2r; d[6]=d4r; d[7]=ac.at(p0); d[8]=bc.at(p0);
d[9]=ac.at(p1); d[10]=bc.at(p2); d[11]=ac.at(p2); d[12]=bc.at(p4); d[13]=ac.at(p4); d[14]=bc.at(p4);
int j = arri(1,d[3],d[4],d[5],d[6],d[7],d[8],d[9],d[10],d[11],d[12],d[13],d[14]);
for(int k=1;k<=j;k++) { iv=0;
p0obj=arr0(k,d[3],d[4],d[5],d[6],d[7],d[8],d[9],d[10],d[11],d[12],d[13],d[14]);
p1obj=arr1(k,d[3],d[4],d[5],d[6],d[7],d[8],d[9],d[10],d[11],d[12],d[13],d[14]);
p2obj=arr2(k,d[3],d[4],d[5],d[6],d[7],d[8],d[9],d[10],d[11],d[12],d[13],d[14]);
//cout<<j<<":"<<p0obj<<"\t"<<p1obj<<"\t"<<p2obj<<"\n"<<p0<<"\t"<<p1<<"\t"<<p2<<endl;
}
}
}
}
}
}
}
}
Above is the main part of my c++ program. Here this step analyzes present object with its coordinates in a,b,c vector and color values in ac,bc,cc vector. Here arri, arr1.. are the maps which contains the index of the point from privious object, here whenever the given point pattern p0,p1,p2,p3,p4 are detected it comes out from the loop. When i run this it is not working as expected and correctly. I have doubt that when the position and oreantation of object is changed the a,b color values of given 4 voxels also changes(from lab color) and its(given two/3/n voxels) distance also somehow do not remains same as previously saved similarly like measurement problem in physics. So my main confusion here is that does the values of a and b changes when the position of pixel/voxel is changed? I am not using L value since the lightness value can change in environment.
2 suggestions:
use approximate distances (a>19 && a<21), (a==20) is incredibly specific and could fail due to digital voxeling noise depending on your resolution and object size.
I would think that you need to check to see if your color matches/ is the correct color to be qualified points, but I see this no where in your code.
If I were writing this... I think my first step would be separating out all pixels inside a certain color range and then I would look through those points for a set of properly spaced items among them.