// C++ [dcl.init.ref]p5b2
void test4() {
double& rd2 = 2.0; // expected-error{{non-const lvalue reference to type 'double' cannot bind to a temporary of type 'double'}}
int i = 2;
double& rd3 = i; // expected-error{{non-const lvalue reference to type 'double' cannot bind to a value of unrelated type 'int'}}
const A& rca = fB();
}
void BulletGeneric6dofConstraint::updateConstructionInfo() {
if (mGeneric6DofConstraint != 0) {
delete (mGeneric6DofConstraint);
}
btRigidBody *rbA = ((BulletRigidBody*)owner_object()->
getComponent(COMPONENT_TYPE_PHYSICS_RIGID_BODY))->getRigidBody();
btVector3 p(mPosition.x, mPosition.y, mPosition.z);
btMatrix3x3 m(mRotationA.vec[0], mRotationA.vec[1], mRotationA.vec[2], mRotationA.vec[3],
mRotationA.vec[4], mRotationA.vec[5], mRotationA.vec[6], mRotationA.vec[7],
mRotationA.vec[8]);
btTransform fA(m, p);
p = rbA->getWorldTransform().getOrigin() + p;
p -= mRigidBodyB->getRigidBody()->getWorldTransform().getOrigin();
m.setValue(mRotationB.vec[0], mRotationB.vec[1], mRotationB.vec[2], mRotationB.vec[3],
mRotationB.vec[4], mRotationB.vec[5], mRotationB.vec[6], mRotationB.vec[7],
mRotationB.vec[8]);
btTransform fB(m, p);
mGeneric6DofConstraint =
new btGeneric6DofConstraint(*rbA, *mRigidBodyB->getRigidBody(), fA, fB, false);
mGeneric6DofConstraint->setLinearLowerLimit(Common2Bullet(mLinearLowerLimits));
mGeneric6DofConstraint->setLinearUpperLimit(Common2Bullet(mLinearUpperLimits));
mGeneric6DofConstraint->setAngularLowerLimit(Common2Bullet(mAngularLowerLimits));
mGeneric6DofConstraint->setAngularUpperLimit(Common2Bullet(mAngularUpperLimits));
mGeneric6DofConstraint->setBreakingImpulseThreshold(mBreakingImpulse);
}
int main (void) {
A2 = fA();
B2 = fB();
C2 = fC();
D2 = fD();
return all (0);
}
void g()
{
mixed_up<A, B, C> m;
fA(m);
fB(m);
fC(m);
}
// C++ [dcl.init.ref]p5b2
void test4() {
double& rd2 = 2.0; // expected-error{{non-const lvalue reference to type 'double' cannot be initialized with a temporary of type 'double'}}
int i = 2;
double& rd3 = i; // expected-error{{non-const lvalue reference to type 'double' cannot be initialized with a value of type 'int'}}
const A& rca = fB();
}
int main (int argc, char *argv[])
{
Extrae_user_function(1);
fA();
fB();
Extrae_user_function(0);
return 0;
}
void u_FP(lt& t, const lo& o) {
ASSERT(fB(o) == t.bI);
if (t.bI == kBN) {
return;
}
uI_FP(t.bI, o);
t.bI = kBN;
}
void lI(lt& t, const lo& o) {
auto bi = fB(o);
auto r = b[bi - 1]->lO(o);
if (r != TLOR::S) {
t.bI = kBN;
return;
}
t.bI = bi;
}
float forceThin(float** arr, const int N, const int M, const int x, const int y, const std::vector<ivec2> borders)
{
vec2 a(x-0.5,y+0.5);
vec2 b(x+0.5,y+0.5);
vec2 c(x+0.5,y-0.5);
vec2 d(x-0.5,y-0.5);
vec2 fA(0.0,0.0);
vec2 fB(0.0,0.0);
vec2 fC(0.0,0.0);
vec2 fD(0.0,0.0);
for (unsigned int i=0; i < borders.size(); ++i)
{
if (distance(ivec2(x,y), borders[i]) <= 16)
{
std::vector<ivec2> line;
line = draw_line(ivec2(x,y), borders[i]);
bool outofsight = false;
//printf("(%d,%d)\n(%d,%d) %zd\n", x, y, borders[i].x, borders[i].y, line.size());
for (unsigned int j=0; j < line.size(); ++j)
{
//printf("\t%d\n", j);
outofsight |= arr[line[j].x][line[j].y] == 0.5;
}
if (!outofsight)
{
float dist = distance(a, borders[i]);
float dx = a.x - borders[i].x;
float dy = a.y - borders[i].y;
fA = fA + vec2(dx/(abs(dx)+abs(dy)) / dist, dy/(abs(dx)+abs(dy)) / dist);
dist = distance(b, borders[i]);
dx = b.x - borders[i].x;
dy = b.y - borders[i].y;
fB = fB + vec2(dx/(abs(dx)+abs(dy)) / dist, dy/(abs(dx)+abs(dy)) / dist);
dist = distance(c, borders[i]);
dx = c.x - borders[i].x;
dy = c.y - borders[i].y;
fC = fC + vec2(dx/(abs(dx)+abs(dy)) / dist, dy/(abs(dx)+abs(dy)) / dist);
dist = distance(d, borders[i]);
dx = d.x - borders[i].x;
dy = d.y - borders[i].y;
fD = fD + vec2(dx/(abs(dx)+abs(dy)) / dist, dy/(abs(dx)+abs(dy)) / dist);
}
}
}
//S |= fA.x*fB.x <= 0.0 && abs(fA.y)+abs(fB.y) > abs(fA.x)+abs(fB.x);
//S |= fC.x*fD.x <= 0.0 && abs(fC.y)+abs(fD.y) > abs(fC.x)+abs(fD.x);
//S |= fA.y*fD.y <= 0.0 && abs(fA.y)+abs(fD.y) > abs(fA.x)+abs(fD.x);
//S |= fB.y*fC.y <= 0.0 && abs(fB.y)+abs(fC.y) > abs(fB.x)+abs(fC.x);
//S |= fA.x*fC.x <= 0.0 && fA.y * fC.y <= 0.0;
//S |= fB.x*fD.x <= 0.0 && fB.y * fD.y <= 0.0;
//S |= fA.x*fB.x <= 0.0 && fC.x*fD.x <= 0.0 && (abs(fA.x)+abs(fB.x) > abs(fA.y)+abs(fB.y) || abs(fC.x)+abs(fD.x) > abs(fC.y)+abs(fD.y));
//S |= fA.y*fD.y <= 0.0 && fB.y*fC.y <= 0.0 && (abs(fA.y)+abs(fD.y) > abs(fA.x)+abs(fD.x) || abs(fB.y)+abs(fC.y) > abs(fB.x)+abs(fC.x));
//S |= fA.x*fC.x <= 0.0 && fA.y*fC.y <= 0.0;
//S |= fB.x*fD.x <= 0.0 && fB.y*fD.y <= 0.0;
//dot products to find change in direction
float dot;
float min_dot = INFINITY;
dot = fA.x*fB.x + fA.y*fB.y;
if (dot < min_dot)
min_dot = dot;
dot = fB.x*fC.x + fB.y*fC.y;
if (dot < min_dot)
min_dot = dot;
dot = fA.x*fD.x + fA.y*fD.y;
if (dot < min_dot)
min_dot = dot;
dot = fC.x*fD.x + fC.y*fD.y;
if (dot < min_dot)
min_dot = dot;
dot = fA.x*fC.x + fA.y*fC.y;
if (dot < min_dot)
min_dot = dot;
dot = fB.x*fD.x + fB.y*fD.y;
if (dot < min_dot)
min_dot = dot;
return min_dot;
}
Vector4 Rgba::ToVector4() const {
return Vector4(fR(), fG(), fB(), fA());
}
void fi::VPDetectionCloud::ClusterKDirectionPatches(const pcl::PointCloud<pcl::PointXYZ>::Ptr &_InCloud, Eigen::VectorXf &fRobustEstimatedVP)
{
unsigned int fNumOfNormals = _InCloud->points.size() * m_ModelParams->fPercentageOfNormals / 100;
unsigned int fNumOfCrossProducts = m_ModelParams->fNumOfCrossProducts;
unsigned int fNumberOfRansacIterations = m_ModelParams->fNumOfIterations;
unsigned int m_k = m_ModelParams->m_k;
if ( m_k == 0)
{
m_k = 5;
}
double fEPS = m_ModelParams->fEPSInDegrees;
Eigen::Vector4f fEstimatedVP1;
//// Create the normal estimation class, and pass the input dataset to it
//pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
//normal_estimation.setInputCloud (cloud);
//// Create an empty kdtree representation, and pass it to the normal estimation object.
//// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
//pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
//normal_estimation.setSearchMethod (tree);
//// Output datasets
//pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
//PtCloudDataPtr InPclDataPtr1(new PtCloudData());
///*pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(pclData);*/
//boost::shared_ptr<PtCloudData> InPclDataPtr(new PtCloudData(pclData));
int nPointCandidates = _InCloud->points.size();
// Consider using more precise timers such as gettimeofday on *nix or
// GetTickCount/timeGetTime/QueryPerformanceCounter on Windows.
boost::mt19937 randGen(std::time(0));
// Now we set up a distribution. Boost provides a bunch of these as well.
// This is the preferred way to generate numbers in a certain range.
// initialize a uniform distribution between 0 and the max=nPointCandidates
boost::uniform_int<> uInt8Dist(0, nPointCandidates);
// Finally, declare a variate_generator which maps the random number
// generator and the distribution together. This variate_generator
// is usable like a function call.
boost::variate_generator< boost::mt19937&, boost::uniform_int<> >
GetRand(randGen, uInt8Dist);
//sample random points and compute their normals
pcl::IndicesPtr indices(new std::vector<int>());
for (unsigned int i = 0; i < fNumOfNormals; i++)
{
indices->push_back(GetRand() % nPointCandidates);
}
/*pcl::NormalEstimationOMP fd;*/ // 6-8 times faster ?
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (_InCloud);
ne.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));
ne.setKSearch (m_k);
//ne.setRadiusSearch (fRadius); //////toDo Set Cloud and radius hier correctly!
//ne.setSearchSurface(source.surface);
ne.setIndices(indices);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>());
ne.compute (*normals);
//typedef boost::scoped_ptr<Eigen::Vector4f> fXProds;
std::vector<boost::shared_ptr<Eigen::Vector4f> > fVPHypothesis;
std::vector<boost::shared_ptr<Eigen::Vector4f> > fVPHypothesisInliers;
//sample random points and compute their normals
for (unsigned int i = 0; i < fNumOfCrossProducts; i++)
{
unsigned int fIndexA = GetRand() % fNumOfNormals;
unsigned int fIndexB = GetRand() % fNumOfNormals;
pcl::Normal &fnormalA = normals->points.at(fIndexA);
pcl::Normal &fnormalB = normals->points.at(fIndexB);
Eigen::Vector4f fA(fnormalA.normal_x, fnormalA.normal_y, fnormalA.normal_z, 0);
Eigen::Vector4f fB(fnormalB.normal_x, fnormalB.normal_y, fnormalB.normal_z, 0);
Eigen::Vector4f fC = fA.cross3(fB);
boost::shared_ptr<Eigen::Vector4f> ftmpRes(new Eigen::Vector4f(fC));
/*fXProds faVPHypothesis(new Eigen::Vector4f(fC));*/
fVPHypothesis.push_back(ftmpRes);
//tmpdistances = sqrt ((line_pt - fPlaneB->points[j].getVector4fMap ()).cross3 (line_dir).squaredNorm ());
}
//The Cross products are then the direction we are looking. Find the majority w.r.t a ref Direction!
Eigen::Vector4f fRefDir (1.0, 0.0, 0.0, 0);
//Validate the best using RANSAC w.r.t refDir
unsigned int counterMax = 0;
unsigned int iBest;
for (unsigned int g = 0; g < fNumberOfRansacIterations; g++)
{
unsigned int gIndex = GetRand() % fNumOfCrossProducts;
Eigen::Vector4f &fu1 = *fVPHypothesis.at(gIndex);
double fAnglesInRadians = pcl::getAngle3D(fu1, fRefDir);
double fAngleInDegrees = pcl::rad2deg(fAnglesInRadians);
//score the selected hypothesis
unsigned int sCounter = 0 ;
for (unsigned int l = 0; l < fNumOfCrossProducts; l++)
{
//const float sAnglesInRadianstmp = Math3d::Angle(*((*crosProds)[l]), sRefDir);
//const float sAnglesInDegreestmp = sAnglesInRadianstmp * 180.0f / CV_PI;
Eigen::Vector4f &fu1tmp = *fVPHypothesis.at(l);
double fAnglesInRadianstmp = pcl::getAngle3D(fu1tmp, fRefDir);
double fAngleInDegreestmp = pcl::rad2deg(fAnglesInRadianstmp);
if (fabsf(fAngleInDegreestmp - fAngleInDegrees) <= fEPS) //Tolerance of 2 Degress!!!
{
sCounter++;
}
}
if (sCounter > counterMax)
{
counterMax = sCounter;
iBest = gIndex;
}
}
//Insert the Estimated VP!
//fEstimatedVP1 = *fVPHypothesis.at(l);
/*delete m_pclDataPtr;*/
//collect all the inliers and do WLeastSquaresFit
Eigen::Vector4f &fBestModel = *fVPHypothesis.at(iBest);
double fAnglesInRadiansBest = pcl::getAngle3D(fBestModel, fRefDir);
double fAngleInDegreesBest = pcl::rad2deg(fAnglesInRadiansBest);
pcl::PointCloud<pcl::PointXYZ>::Ptr fVPCandidates(new pcl::PointCloud<pcl::PointXYZ>);
fVPCandidates->width = counterMax;
fVPCandidates->height = 1;
fVPCandidates->resize(fVPCandidates->width * fVPCandidates->height);
unsigned int ii = 0;
for (unsigned int l = 0; l < fNumOfCrossProducts; l++)
{
Eigen::Vector4f &fu1tmp = *fVPHypothesis.at(l);
double fAnglesInRadianstmp = pcl::getAngle3D(fu1tmp, fRefDir);
double fAngleInDegreestmp = pcl::rad2deg(fAnglesInRadianstmp);
if (fabsf(fAngleInDegreestmp - fAngleInDegreesBest) <= fEPS) //Tolerance of 2 Degrees!!!
{
fVPCandidates->points[ii].x = fu1tmp(0);
fVPCandidates->points[ii].y = fu1tmp(1);
fVPCandidates->points[ii].z = fu1tmp(2);
/*boost::scoped_ptr<Eigen::Vector4f> ftmpInliers(new Eigen::Vector4f(fu1tmp));
fVPHypothesisInliers.push_back(ftmpInliers);*/
if (ii > counterMax)
{
std::cout<<" Something went really wrong!..."<<std::endl;
return;
}
ii++;
}
}
//Robust fitting the inliers using WleastSquears fit
//Eigen::VectorXf optimized_vp_coefficients;
LSLineFitting(fVPCandidates, fRobustEstimatedVP);
//collect data for
}
