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); } }