TEST_F(FormulateILPTest, testSingletonConstraint) {
auto command = Utility::getPointer(new SingletonConstraintFormulateCommand(*graph, *grid));
auto equations = command->formulate();
std::set<std::string> equationStrings;
for (auto equation : *equations) {
equationStrings.insert(equation->toString());
}
EXPECT_EQ(1, equationStrings.size());
EXPECT_TRUE(equationStrings.find("module1_0_0_0_0+module1_0_0_1_0+module1_0_0_0_1=1") != equationStrings.end());
}
float Vector::dotProduct(Vector& Vt)
{
formulate();
Vt.formulate();
float t=0.0f;
int i;
for (i=0;i<3;i++)
t += vm[i] * Vt.vm[i];
return t;
}
void Vector::normInNewSpace(float ssx,float ssy, float ssz)
{//ssx,ssy,ssz is multiplied to change Vector to the current space
//this is to get a vector in current space equal to a normlized
// in the original space
formulate();
ssx *=ssx; ssy *= ssy; ssz *= ssz;
double t = sqrt(vm[0]*vm[0]/ssz+vm[1]*vm[1]/ssy+vm[2]*vm[2]/ssz);
deScale(t,t,t);
}
float Vector::length()
{
int i;
double t=0.0f;
formulate();
for (i=0;i<3;i++) t += vm[i]*vm[i];
//t=_sqrt_s(t);//fabs(vm[3]);
t=sqrt(t);
return t;
}
bool MIQPSolver::Formulate(const DataStore &store, const vector<double> &coord)
{
if (status != Clean)
return false;
if (!formulate(store) || !addCoordinateConstraints(coord) || !createModel())
{
status = Fail;
return false;
}
return true;
}
Vector Vector::operator*(float dd)
{
Vector tmp;
formulate();
tmp.vm[0] = dd * vm[0];
tmp.vm[1] = dd * vm[1];
tmp.vm[2] = dd * vm[2];
return tmp;
}
bool MIQPSolver::Formulate(const DataStore &store)
{
if (status != Clean)
return false;
if (!formulate(store) || !createModel())
{
status = Fail;
return false;
}
status = Formulated;
return true;
}
Vector Vector::operator/(float dd)
{
Vector tmp;
formulate();
if (fabs(dd-0.0f)>ZERO) {
tmp.setX( vm[0] / dd );
tmp.setY( vm[1] / dd );
tmp.setZ( vm[2] / dd );
}
return tmp;
}
Vector Vector::operator-(Vector vt)
{
Vector tmp;
int i;
formulate();
vt.formulate();
for (i=0;i<3;i++)
tmp.vm[i] = vm[i] - vt.vm[i];
return tmp;
}
Vector Vector::vectorRightProduct(Vector& Vt)
{
Vector T;
formulate();
Vt.formulate();
T.vm[0] = vm[1]*Vt.vm[2] - vm[2]*Vt.vm[1];
T.vm[1] = vm[2]*Vt.vm[0] - vm[0]*Vt.vm[2];
T.vm[2] = vm[0]*Vt.vm[1] - vm[1]*Vt.vm[0];
T.vm[3] = 1.0f;
return T;
}
void Vector::norm()
{
formulate();
double t=length();
//cout <<" $ ";
if (fabs(t)<ZERO){
cout<<"\nERROR:Vector::norm() failed,t="<<t<<endl;
cout<<"\n (XYZ)=("<< vm[0]<<","<<vm[1]<<","<<vm[2]<<")"<<endl;
setXYZ(0.0f,0.0f,0.0f);
}else{
vm[0] /= t;
vm[1] /= t;
vm[2] /= t;
}
}
TEST_F(FormulateILPTest, testNonOverlappingConstraint) {
ModuleNodePointer module = Utility::getPointer(new RectangularModuleFactory("module2"));
module->addShape(Utility::getPointer(new RectangularShape("0", Coord(1, 1, 1))));
graph->add(module);
auto command = Utility::getPointer(new NonOverlappingConstraintFormulateCommand(*graph, *grid));
auto equations = command->formulate();
std::set<std::string> equationStrings;
for (auto equation : *equations) {
equationStrings.insert(equation->toString());
}
EXPECT_EQ(2, equationStrings.size());
EXPECT_TRUE(equationStrings.find("module1_0_0_0_0+module1_0_0_0_1+module2_0_0_0_0<=1") != equationStrings.end());
EXPECT_TRUE(equationStrings.find("module1_0_0_1_0+module1_0_0_0_1+module2_0_0_1_0<=1") != equationStrings.end());
}
float fPoint::distance(fPoint pt)
{
int i;
float t=0.0f,tt;
formulate();
pt.formulate();
for (i=0;i<3;i++){
tt = fabs(vm[i]-pt.vm[i]);
t += tt * tt;
}
//t=_sqrt_s(t);//fabs(vm[3]);
t=sqrt(t);
return t;
}
int Vector::normJudge()
{
formulate();
double t=length();
if (fabs(t)<ZERO){
//printf("\nERROR:Vector::norm() failed");
setXYZ(0.0f,0.0f,0.0f);
return -1;
}else{
vm[0] /= t;
vm[1] /= t;
vm[2] /= t;
return 1;
}
}
fPoint fPoint::proportionPoint(fPoint B, float t1, float t2)
{
fPoint tmp;
if ((t1+t2-0)<ZERO){// r= -1, insection point at infinative place
printf ("\n ERROR: Image::proportion: viewpoint on object.");
exit(0);
}
if ((t2-0)<ZERO){//it is the same as viewpoint
printf("\n ERROR: Image::dotOnImagePlane: viewpoint on view Plane");
return B;
}
float r = t1 / t2;
formulate(); B.formulate();
int i;
for (i=0;i<3; i++)
tmp.vm[i] = (vm[i] + r * B.vm[i])/(1+r);
tmp.vm[3] = 1.0f;
return tmp;
}
void fPoint::deScale(float sx,float sy, float sz)
{
formulate();
vm[0] /= sx; vm[1] /= sy; vm[2] /= sz;
}
void fPoint::getXYZ(float& a,float& b, float& c)
{
formulate();
a=vm[0];b=vm[1];c=vm[2];
}
int main( int argc, char *argv[] )
{
if ( (argc == 2) &&
( (GF2::console::find_switch(argc,argv,"--help"))
|| (GF2::console::find_switch(argc,argv,"-h" ))
)
)
{
std::cout << "[Usage]:\n"
<< "\t--generate\n"
<< "\t--generate3D\n"
<< "\t--formulate\n"
<< "\t--formulate3D\n"
<< "\t--solver mosek|bonmin|gurobi\n"
<< "\t--solver3D mosek|bonmin|gurobi\n"
<< "\t--merge\n"
<< "\t--merge3D\n"
<< "\t--datafit\n"
<< "\t--corresp\n"
//<< "\t--show\n"
<< std::endl;
return EXIT_SUCCESS;
}
else if ( GF2::console::find_switch(argc,argv,"--segment") || GF2::console::find_switch(argc,argv,"--segment3D") )
{
return segment( argc, argv );
}
else if ( GF2::console::find_switch(argc,argv,"--generate") || GF2::console::find_switch(argc,argv,"--generate3D") )
{
return generate(argc,argv);
}
else if ( GF2::console::find_switch(argc,argv,"--formulate") || GF2::console::find_switch(argc,argv,"--formulate3D"))
{
return formulate( argc, argv );
//return GF2::ProblemSetup::formulateCli<GF2::Solver::PrimitiveContainerT, GF2::Solver::PointContainerT>( argc, argv );
}
else if ( GF2::console::find_switch(argc,argv,"--solver") || GF2::console::find_switch(argc,argv,"--solver3D") ) // Note: "solver", not "solve" :-S
{
return solve( argc, argv );
//return GF2::Solver::solve( argc, argv );
}
else if ( GF2::console::find_switch(argc,argv,"--datafit") || GF2::console::find_switch(argc,argv,"--datafit3D") )
{
return datafit( argc, argv );
//return GF2::Solver::datafit( argc, argv );
}
else if ( GF2::console::find_switch(argc,argv,"--merge") || GF2::console::find_switch(argc,argv,"--merge3D") )
{
return merge(argc, argv);
}
else if ( GF2::console::find_switch(argc,argv,"--show") )
{
std::cerr << "[" << __func__ << "]: " << "the show option has been moved to a separate executable, please use thatt one" << std::endl;
return 1;
//return GF2::Solver::show( argc, argv );
}
else if ( GF2::console::find_switch(argc,argv,"--subsample") )
{
return subsample( argc, argv );
}
// else if ( GF2::console::find_switch(argc,argv,"--corresp") || GF2::console::find_switch(argc,argv,"--corresp3D") )
// {
// return corresp( argc, argv );
// }
std::cerr << "[" << __func__ << "]: " << "unrecognized option" << std::endl;
return 1;
// --show --dir . --cloud cloud.ply --scale 0.05f --assoc points_primitives.txt --use-tags --no-clusters --prims primitives.bonmin.txt
// std::string img_path( "input2.png" );
// pcl::console::parse_argument( argc, argv, "--img", img_path );
// float scale = 0.1f;
// pcl::console::parse_argument( argc, argv, "--scale", scale );
// return GF2::Solver::run( img_path, scale, {0, M_PI_2, M_PI}, argc, argv );
}
void fPoint::scale(float sx,float sy, float sz)
{
formulate();
vm[0] *= sx; vm[1] *= sy; vm[2] *= sz;
}
Vector Vector::vvScale(Vector ad)
{
formulate(); ad.formulate();
return Vector(vm[0]*ad.vm[0], vm[1]*ad.vm[1], vm[2]*ad.vm[2]);
}
