void setupClouds(string name, vector<cv::Mat> &images, vector<PointCloud<PointXYZRGB>::Ptr> &allPtClouds){
//cout << "k: " << k << endl;
cv::Mat ori;
double xc,yc,zc;
PointCloud<PointXYZRGB>::Ptr cloud (new PointCloud<PointXYZRGB>);
for(int k=0;k<nb_images;k++){
ori = images[k];
//loadImagei(name,k+1,ori);
//allImages[k] = ori;
//cout << "width: " << allImages[k].cols << endl;
//cout << "Height: " << allImages[k].rows << endl;
//Verify image has content
if(!ori.data){
cout << "Input image was not loaded, please verify: " << name << k <<".jpg" << endl;
return ;
}
//Get the center of the Kth sphere in World Coordinates
sphereCenter(alpha, k, dist_c, xc, zc);
//Create Sphere from Equirectangular image and store in Point Cloud
yc=0;
cloud = EquiToSphere(ori, radius,xc,yc,zc);
allPtClouds[k] = cloud;
}
}
TEST(CapsuleSphereContactCalculationTests, UnitTests)
{
{
SCOPED_TRACE("No Intersection");
doCapsuleSphereTest(0.2, 0.1, Vector3d::Zero(), Quaterniond::Identity(),
0.1, Vector3d(1.0, 1.0, 1.0), Quaterniond::Identity(), false, 0.0);
}
{
SCOPED_TRACE("Capsule along Y-axis, intersection with cylindrical part of the capsule");
doCapsuleSphereTest(0.8, 0.5, Vector3d::Zero(), Quaterniond::Identity(),
0.3, Vector3d(0.7, 0, 0), Quaterniond::Identity(), true, 0.1,
Vector3d::Zero(), Vector3d(-1.0, 0.0, 0.0));
}
{
SCOPED_TRACE("Capsule along X-axis, intersection with hemispherical part of the capsule");
doCapsuleSphereTest(0.1, 0.2, Vector3d::Zero(),
SurgSim::Math::makeRotationQuaternion(M_PI_2, Vector3d(0.0, 0.0, 1.0)),
0.1, Vector3d(-0.2, 0.0, 0.0),
Quaterniond::Identity(), true, 0.15,
Vector3d(0.0, 0.05, 0.0), Vector3d(1.0, 0.0, 0.0));
}
{
SCOPED_TRACE("Intersection, capsule roated along Z-axis clockwise 45 degrees");
Vector3d sphereCenter(2.0, 0.0, 0.0);
Vector3d sphereProjection(1.0, 1.0, 0.0);
Vector3d expectedNormal = (sphereProjection - sphereCenter).normalized();
doCapsuleSphereTest(2 * M_SQRT2, M_SQRT2, Vector3d::Zero(),
SurgSim::Math::makeRotationQuaternion(-M_PI_4, Vector3d(0.0, 0.0, 1.0)),
1.0, sphereCenter,
Quaterniond::Identity(), true, 1,
Vector3d(0.0, M_SQRT2, 0.0), expectedNormal);
}
}
