/*!
Check if the polygon is visible in the image and if the angle between the normal
to the face and the line vector going from the optical center to the cog of the face is below
the given threshold.
To do that, the polygon is projected into the image thanks to the camera pose.
\param cMo : The pose of the camera.
\param alpha : Maximum angle to detect if the face is visible (in rad).
\param modulo : Indicates if the test should also consider faces that are not oriented
counter clockwise. If true, the orientation of the face is without importance.
\param cam : Camera parameters (intrinsics parameters)
\param I : Image used to consider level of detail.
\return Return true if the polygon is visible.
*/
bool
vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo,
const vpCameraParameters &cam, const vpImage<unsigned char> &I)
{
// std::cout << "Computing angle from MBT Face (cMo, alpha)" << std::endl;
changeFrame(cMo);
if(nbpt <= 2) {
//Level of detail (LOD)
if(useLod) {
vpCameraParameters c = cam;
if(clippingFlag > 3) { // Contains at least one FOV constraint
c.computeFov(I.getWidth(), I.getHeight());
}
computePolygonClipped(c);
std::vector<vpImagePoint> roiImagePoints;
getRoiClipped(c, roiImagePoints);
if (roiImagePoints.size() == 2) {
double x1 = roiImagePoints[0].get_u();
double y1 = roiImagePoints[0].get_v();
double x2 = roiImagePoints[1].get_u();
double y2 = roiImagePoints[1].get_v();
double length = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
// std::cout << "Index=" << index << " ; Line length=" << length << " ; clippingFlag=" << clippingFlag << std::endl;
// vpTRACE("index=%d lenght=%f minLineLengthThresh=%f", index, length, minLineLengthThresh);
if (length < minLineLengthThresh) {
isvisible = false;
isappearing = false;
return false;
}
}
}
/* a line is always visible when LOD is not used */
isvisible = true;
isappearing = false;
return true ;
}
// If the polygon has no orientation, the angle check visibility is always valid.
// Feature mainly used for cylinders.
if(!hasOrientation)
{
isvisible = true;
isappearing = false;
return true;
}
//Check visibility from normal
//Newell's Method for calculating the normal of an arbitrary 3D polygon
//https://www.opengl.org/wiki/Calculating_a_Surface_Normal
vpColVector faceNormal(3);
vpColVector currentVertex, nextVertex;
for(unsigned int i = 0; i<nbpt; i++) {
currentVertex = p[i].cP;
nextVertex = p[(i+1) % nbpt].cP;
faceNormal[0] += (currentVertex[1] - nextVertex[1]) * (currentVertex[2] + nextVertex[2]);
faceNormal[1] += (currentVertex[2] - nextVertex[2]) * (currentVertex[0] + nextVertex[0]);
faceNormal[2] += (currentVertex[0] - nextVertex[0]) * (currentVertex[1] + nextVertex[1]);
}
faceNormal.normalize();
vpColVector e4(3) ;
vpPoint pt;
for (unsigned int i = 0; i < nbpt; i += 1){
pt.set_X(pt.get_X() + p[i].get_X());
pt.set_Y(pt.get_Y() + p[i].get_Y());
pt.set_Z(pt.get_Z() + p[i].get_Z());
}
e4[0] = -pt.get_X() / (double)nbpt;
e4[1] = -pt.get_Y() / (double)nbpt;
e4[2] = -pt.get_Z() / (double)nbpt;
e4.normalize();
double angle = acos(vpColVector::dotProd(e4, faceNormal));
// vpCTRACE << angle << "/" << vpMath::deg(angle) << "/" << vpMath::deg(alpha) << std::endl;
if( angle < alpha || (modulo && (M_PI - angle) < alpha)) {
isvisible = true;
isappearing = false;
if (useLod) {
vpCameraParameters c = cam;
if(clippingFlag > 3) { // Contains at least one FOV constraint
c.computeFov(I.getWidth(), I.getHeight());
}
computePolygonClipped(c);
std::vector<vpImagePoint> roiImagePoints;
getRoiClipped(c, roiImagePoints);
vpPolygon roiPolygon(roiImagePoints);
double area = roiPolygon.getArea();
// std::cout << "After normal test ; Index=" << index << " ; area=" << area << " ; clippingFlag="
// << clippingFlag << std::endl;
if (area < minPolygonAreaThresh) {
isappearing = false;
isvisible = false;
return false;
}
}
return true;
}
if (angle < alpha+vpMath::rad(1) ){
isappearing = true;
}
else if (modulo && (M_PI - angle) < alpha+vpMath::rad(1) ){
isappearing = true;
}
else {
isappearing = false;
}
isvisible = false ;
return false ;
}
int json_cpp_tests() {
json::Value e1(json::load_file("test.json"));
json::Value e2(e1);
json::Value e3;
json::Value e4(json::load_string("{\"foo\": true, \"bar\": \"test\"}"));
ASSERT_TRUE(e1.is_object(), "e1 is not an object");
ASSERT_TRUE(e2.is_object(), "e2 is not an object");
ASSERT_TRUE(e3.is_undefined(), "e3 has a defined value");
ASSERT_TRUE(e4.is_object(), "e4 is not an object");
ASSERT_EQ(e1.size(), 1, "e1 has too many properties");
ASSERT_EQ(e2.size(), 1, "e2 has too many properties");
ASSERT_EQ(e4.size(), 2, "e4 does not have 2 elements");
ASSERT_TRUE(e1.get("web-app").is_object(), "e1[0].web-app is not an object");
ASSERT_EQ(e1.get("web-app").get("servlet").at(0).get("servlet-class").as_string(), "org.cofax.cds.CDSServlet", "property has incorrect value");
ASSERT_EQ(e1["web-app"]["servlet"][0]["servlet-class"].as_string(), "org.cofax.cds.CDSServlet", "property has incorrect value");
ASSERT_EQ(e4["foo"].as_boolean(), true, "property has incorrect value");
// verify iterator results (note that they can be returned in any order)
json::Iterator i(e1.get("web-app"));
std::set<std::string> iteratorResults;
for ( int ii = 0; ii < 3; ++ii ) {
ASSERT_FALSE(i.key().empty(), "iterator returned a null value");
iteratorResults.insert(i.key());
i.next();
}
ASSERT_FALSE(i.valid(), "iterator has more values than expected");
ASSERT_EQ(iteratorResults.size(), 3, "iterator did not return enough values");
json::Value e5(json::Value(12.34));
ASSERT_TRUE(e5.is_number(), "e5 is not a number after assignment");
ASSERT_EQ(e5.as_real(), 12.34, "e5 has incorrect value after assignment");
json::Value e6(json::Value(true));
ASSERT_TRUE(e6.is_boolean(), "e6 is not a boolean after assignment");
ASSERT_EQ(e6.as_boolean(), true, "e6 has incorrect value after assignment");
json::Value e7(json::Value("foobar"));
ASSERT_TRUE(e7.is_string(), "e7 is not a string after assignment");
ASSERT_EQ(e7.as_string(), "foobar", "e7 has incorrect value after assignment");
json::Value e8(json::object());
ASSERT_TRUE(e8.is_object(), "e8 is not an object after assignment");
json::Value e9(json::null());
ASSERT_TRUE(e9.is_null(), "e9 is not null after assignment");
json::Value e10(json::array());
ASSERT_TRUE(e10.is_array(), "e10 is not an array after index assignment");
e10.set_at(0, json::Value("foobar"));
ASSERT_EQ(e10.size(), 1, "e10 has incorrect number of elements after assignment");
ASSERT_EQ(e10[0].as_string(), "foobar", "e10[0] has incorrect value after assignment");
e10.set_at(1, json::Value("foobar"));
ASSERT_TRUE(e10.is_array(), "e10 is not an array after index assignment");
ASSERT_EQ(e10.size(), 2, "e10 has incorrect number of elements after assignment");
ASSERT_EQ(e10[1].as_string(), "foobar", "e10[0] has incorrect value after assignment");
e10.set_at(0, json::Value("barfoo"));
ASSERT_TRUE(e10.is_array(), "e10 is not an array after index assignment");
ASSERT_EQ(e10.size(), 2, "e10 has incorrect number of elements after assignment");
ASSERT_EQ(e10[0].as_string(), "barfoo", "e10[0] has incorrect value after assignment");
e10.set_at(100, json::null());
ASSERT_TRUE(e10.is_array(), "e10 is not an array after index assignment");
ASSERT_EQ(e10.size(), 2, "e10 has incorrect number of elements after assignment");
e10.insert_at(1, json::Value("new"));
ASSERT_EQ(e10.size(), 3, "e10 has incorrect size after insert");
ASSERT_EQ(e10[1].as_string(), "new", "e10[1] has incorrect value after insert");
ASSERT_EQ(e10[2].as_string(), "foobar", "e10[2] has incorrect value after insert");
e10.del_at(0);
ASSERT_EQ(e10.size(), 2, "e10 has incorrect size after delete");
ASSERT_EQ(e10[1].as_string(), "foobar", "e10[1] has incorrect value after delete");
e10.clear();
ASSERT_EQ(e10.size(), 0, "e10 has incorrect number of elements after clear");
json::Value e11(json::object());
ASSERT_TRUE(e11.is_object(), "e11 is not an object after property assignment");
e11.set_key("foo", json::Value("test"));
ASSERT_EQ(e11.size(), 1, "e11 has incorrect number of properties after assignment");
ASSERT_EQ(e11["foo"].as_string(), "test", "e11.foo has incorrect value after assignment");
e11.set_key("foo", json::Value("again"));
ASSERT_TRUE(e11.is_object(), "e11 is not an object after property assignment");
ASSERT_EQ(e11.size(), 1, "e11 has incorrect number of properties after assignment");
ASSERT_EQ(e11["foo"].as_string(), "again", "e11.foo has incorrect value after assignment");
e11.set_key("bar", json::Value("test"));
ASSERT_TRUE(e11.is_object(), "e11 is not an object after property assignment");
ASSERT_EQ(e11.size(), 2, "e11 has incorrect number of properties after assignment");
ASSERT_EQ(e11["bar"].as_string(), "test", "e11.foo has incorrect value after assignment");
e11.clear();
ASSERT_EQ(e11.size(), 0, "e11 has incorrect number of properties after clear");
json::Value e12(json::object());
e12.set_key("foo", json::Value("test"));
e12.set_key("bar", json::Value(3));
char* out_cstr = e12.save_string(JSON_COMPACT);
std::string out(out_cstr);
free(out_cstr);
ASSERT_EQ(out, "{\"bar\":3,\"foo\":\"test\"}", "object did not serialize as expected");
std::istringstream instr(out);
instr >> e12;
ASSERT_TRUE(e12.is_object(), "e12 is not an object after stream read");
ASSERT_EQ(e12.size(), 2, "e12 has wrong size after stream read");
ASSERT_EQ(e12.get("bar").as_integer(), 3, "e12.bar has incorrect value after stream read");
ASSERT_EQ(e12.get("foo").as_string(), "test", "ee12.test has incorrect value after stream read");
std::ostringstream outstr;
outstr << e12;
ASSERT_EQ(instr.str(), "{\"bar\":3,\"foo\":\"test\"}", "object did not serialize as expected");
const json::Value e13(e12);
ASSERT_EQ(e13["bar"].as_integer(), 3, "e13.bar has incorrect value after copy");
json::Value e14(json::object());
ASSERT_TRUE(e14.is_object(), "e14 is not an object after construction");
e14.set_key("foo", json::object());
ASSERT_TRUE(e14["foo"].is_object(), "e14.foo is not an object after assignment");
e14["foo"]["bar"] = json::Value(42);
ASSERT_EQ(e14["foo"]["bar"].as_integer(), 42, "e14.foo.bar has incorrect value after assignment");
json::Value e15(json::array());
ASSERT_TRUE(e15.is_array(), "e15 is not an array after construction");
e15.set_at(0, json::Value(42));
ASSERT_EQ(e15[0].as_integer(), 42, "e15[0] has incorrect value after assignment");
e15[0] = json::Value("foo");
ASSERT_EQ(e15[0].as_string(), "foo", "e15[0] has incorrecy value after assignment");
return 0;
}
