TEST(Estimator,EssentialFivePoint) { auto estimator=GSLAM::Estimator::create(); if(!estimator.get()) { LOG(WARNING)<<"Test aborded since estimator not created."; return ; } const double points1_raw[] = { 0.4964, 1.0577, 0.3650, -0.0919, -0.5412, 0.0159, -0.5239, 0.9467, 0.3467, 0.5301, 0.2797, 0.0012, -0.1986, 0.0460, -0.1622, 0.5347, 0.0796, 0.2379, -0.3946, 0.7969, 0.2, 0.7, 0.6, 0.3}; const double points2_raw[] = { 0.7570, 2.7340, 0.3961, 0.6981, -0.6014, 0.7110, -0.7385, 2.2712, 0.4177, 1.2132, 0.3052, 0.4835, -0.2171, 0.5057, -0.2059, 1.1583, 0.0946, 0.7013, -0.6236, 3.0253, 0.5, 0.9, 0.9, 0.2}; const size_t kNumPoints = 12; std::vector<GSLAM::Point2d> points1(kNumPoints); std::vector<GSLAM::Point2d> points2(kNumPoints); for (size_t i = 0; i < kNumPoints; ++i) { points1[i] = GSLAM::Point2d(points1_raw[2 * i], points1_raw[2 * i + 1]); points2[i] = GSLAM::Point2d(points2_raw[2 * i], points2_raw[2 * i + 1]); } double F[9]; std::vector<uchar> inlier_mask; if(!estimator->findEssentialMatrix(F,points1,points2,GSLAM::RANSAC,0.02,0.9999,&inlier_mask)) return ; EXPECT_FALSE(inlier_mask[10]); EXPECT_FALSE(inlier_mask[11]); }
TEST(Mesh,CompareRealSpace) { alps::gf::real_space_index_mesh::container_type points1(boost::extents[20][3]); alps::gf::real_space_index_mesh::container_type points2(boost::extents[20][3]); alps::gf::real_space_index_mesh::container_type points3(boost::extents[20][3]); alps::gf::real_space_index_mesh::container_type points4(boost::extents[3][20]); for (int i=0; i<points1.num_elements(); ++i) { *(points1.origin()+i)=i; *(points2.origin()+i)=i; *(points3.origin()+i)=i+1; *(points4.origin()+i)=i; } alps::gf::real_space_index_mesh mesh1(points1); alps::gf::real_space_index_mesh mesh2(points2); alps::gf::real_space_index_mesh mesh3(points3); alps::gf::real_space_index_mesh mesh4(points4); EXPECT_TRUE(mesh1==mesh2); EXPECT_TRUE(mesh1!=mesh3); EXPECT_TRUE(mesh1!=mesh4); EXPECT_FALSE(mesh1==mesh3); EXPECT_FALSE(mesh1!=mesh2); EXPECT_FALSE(mesh1==mesh4); }
TEST(Estimator,FundamentalEightPoint) { auto estimator=GSLAM::Estimator::create(); if(!estimator.get()) { LOG(WARNING)<<"Test aborded since estimator not created."; return ; } const double points1_raw[] = {1.839035, 1.924743, 0.543582, 0.375221, 0.473240, 0.142522, 0.964910, 0.598376, 0.102388, 0.140092, 15.994343, 9.622164, 0.285901, 0.430055, 0.091150, 0.254594}; const double points2_raw[] = { 1.002114, 1.129644, 1.521742, 1.846002, 1.084332, 0.275134, 0.293328, 0.588992, 0.839509, 0.087290, 1.779735, 1.116857, 0.878616, 0.602447, 0.642616, 1.028681, }; const size_t kNumPoints = 8; std::vector<GSLAM::Point2d> points1(kNumPoints); std::vector<GSLAM::Point2d> points2(kNumPoints); for (size_t i = 0; i < kNumPoints; ++i) { points1[i] = GSLAM::Point2d(points1_raw[2 * i], points1_raw[2 * i + 1]); points2[i] = GSLAM::Point2d(points2_raw[2 * i], points2_raw[2 * i + 1]); } double F[9]; EXPECT_TRUE(estimator->findFundamental(F,points1,points2,GSLAM::FM_8POINT)); // Reference values obtained from Matlab. for(int i=0;i<9;i++) F[i]*=0.0221019; EXPECT_LE(fabs(-0.217859-F[0]), 2e-2); EXPECT_LE(fabs(0.419282-F[1]), 2e-2);// TODO : why opencv impementation not pass <1-5 EXPECT_LE(fabs(-0.0343075-F[2]), 1e-2); EXPECT_LE(fabs(-0.0717941-F[3]), 1e-2); EXPECT_LE(fabs(0.0451643-F[4]), 1e-2); EXPECT_LE(fabs(0.0216073-F[5]), 1e-2); EXPECT_LE(fabs(0.248062-F[6]), 2e-2); EXPECT_LE(fabs(-0.429478-F[7]), 2e-2); EXPECT_LE(fabs(0.0221019-F[8]), 1e-2); }
TEST(Estimator,FundamentalSevenPoint) { auto estimator=GSLAM::Estimator::create(); if(!estimator.get()) { LOG(WARNING)<<"Test aborded since estimator not created."; return ; } const double points1_raw[] = {0.4964, 1.0577, 0.3650, -0.0919, -0.5412, 0.0159, -0.5239, 0.9467, 0.3467, 0.5301, 0.2797, 0.0012, -0.1986, 0.0460}; const double points2_raw[] = {0.7570, 2.7340, 0.3961, 0.6981, -0.6014, 0.7110, -0.7385, 2.2712, 0.4177, 1.2132, 0.3052, 0.4835, -0.2171, 0.5057}; const size_t kNumPoints = 7; std::vector<GSLAM::Point2d> points1(kNumPoints); std::vector<GSLAM::Point2d> points2(kNumPoints); for (size_t i = 0; i < kNumPoints; ++i) { points1[i] = GSLAM::Point2d(points1_raw[2 * i], points1_raw[2 * i + 1]); points2[i] = GSLAM::Point2d(points2_raw[2 * i], points2_raw[2 * i + 1]); } double F[9]; EXPECT_TRUE(estimator->findFundamental(F,points1,points2,GSLAM::FM_7POINT)); // Reference values obtained from Matlab. EXPECT_LE(fabs(F[0]-4.81441976), 1e-6); EXPECT_LE(fabs(F[1]+8.16978909), 1e-6); EXPECT_LE(fabs(F[2]-6.73133404), 1e-6); EXPECT_LE(fabs(5.16247992-F[3]), 1e-6); EXPECT_LE(fabs(0.19325606-F[4]), 1e-6); EXPECT_LE(fabs(-2.87239381-F[5]), 1e-6); EXPECT_LE(fabs(-9.92570126-F[6]), 1e-6); EXPECT_LE(fabs(3.64159554-F[7]), 1e-6); EXPECT_LE(fabs(1.-F[8]), 1e-6); }
void prob2a(){ std::cout << "Problem 2 Part A ======== " << std::endl; Matrix points1(10000,std::vector<double>(2)); Matrix points2(10000,std::vector<double>(2)); loadFile(points1, "sample_data/output1"); loadFile(points2, "sample_data/output3"); std::vector<double> mean1 = getSampleMean(points1); std::vector<double> mean2 = getSampleMean(points2); std::cout << "sample_mean1 = "; print_vec(mean1); std::cout << "sample_mean2 = "; print_vec(mean2); Matrix cov1 = getSampleVar(points1, mean1); Matrix cov2 = getSampleVar(points2, mean2); std::cout << "sample_cov1 = "; print_matrix(cov1); std::cout << "sample_cov2 = "; print_matrix(cov2); std::cout << "With estimated params: " << std::endl; QuadraticDiscriminant classifier1(mean1, mean2, cov1, cov2); calcError(classifier1,points1,points2, "sample_data/labels1"); std::cout << "Given params: " << std::endl; mean1 = {1.0,1.0}; mean2 = {6.0,6.0}; cov1 = {{2.0,0},{0,2.0}}; cov2 = {{4.0,0},{0,8.0}}; QuadraticDiscriminant classifier2(mean1, mean2, cov1, cov2); calcError(classifier2,points1,points2, "sample_data/labels1"); std::cout << std::endl; }
void create_curve_rebar(IfcHierarchyHelper& file) { int dia = 24; int R = 3 * dia; int length = 12 * dia; double crossSectionarea = M_PI * (dia / 2) * 2; IfcSchema::IfcReinforcingBar* rebar = new IfcSchema::IfcReinforcingBar( guid(), 0, S("test"), null, null, 0, 0, null, S("SR24"), //SteelGrade dia, //diameter crossSectionarea, //crossSectionarea = math.pi*(12.0/2)**2 0, IfcSchema::IfcReinforcingBarRoleEnum::IfcReinforcingBarRoleEnum::IfcReinforcingBarRole_LIGATURE, IfcSchema::IfcReinforcingBarSurfaceEnum::IfcReinforcingBarSurfaceEnum::IfcReinforcingBarSurface_PLAIN //PLAIN or TEXTURED ); file.addBuildingProduct(rebar); rebar->setOwnerHistory(file.getSingle<IfcSchema::IfcOwnerHistory>()); IfcSchema::IfcCompositeCurveSegment::list::ptr segments(new IfcSchema::IfcCompositeCurveSegment::list()); IfcSchema::IfcCartesianPoint* p1 = file.addTriplet<IfcSchema::IfcCartesianPoint>(0, 0, 1000.); IfcSchema::IfcCartesianPoint* p2 = file.addTriplet<IfcSchema::IfcCartesianPoint>(0, 0, 0); IfcSchema::IfcCartesianPoint* p3 = file.addTriplet<IfcSchema::IfcCartesianPoint>(0, R, 0); IfcSchema::IfcCartesianPoint* p4 = file.addTriplet<IfcSchema::IfcCartesianPoint>(0, R, -R); IfcSchema::IfcCartesianPoint* p5 = file.addTriplet<IfcSchema::IfcCartesianPoint>(0, R + length, -R); /*first segment - line */ IfcSchema::IfcCartesianPoint::list::ptr points1(new IfcSchema::IfcCartesianPoint::list()); points1->push(p1); points1->push(p2); file.addEntities(points1->generalize()); IfcSchema::IfcPolyline* poly1 = new IfcSchema::IfcPolyline(points1); file.addEntity(poly1); IfcSchema::IfcCompositeCurveSegment* segment1 = new IfcSchema::IfcCompositeCurveSegment(IfcSchema::IfcTransitionCode::IfcTransitionCode_CONTINUOUS, true, poly1); file.addEntity(segment1); segments->push(segment1); /*second segment - arc */ IfcSchema::IfcAxis2Placement3D* axis1 = new IfcSchema::IfcAxis2Placement3D(p3, file.addTriplet<IfcSchema::IfcDirection>(1, 0, 0), file.addTriplet<IfcSchema::IfcDirection>(0, 1, 0)); file.addEntity(axis1); IfcSchema::IfcCircle* circle = new IfcSchema::IfcCircle(axis1, R); file.addEntity(circle); IfcEntityList::ptr trim1(new IfcEntityList); IfcEntityList::ptr trim2(new IfcEntityList); trim1->push(new IfcSchema::IfcParameterValue(180)); trim1->push(p2); trim2->push(new IfcSchema::IfcParameterValue(270)); trim2->push(p4); IfcSchema::IfcTrimmedCurve* trimmed_curve = new IfcSchema::IfcTrimmedCurve(circle, trim1, trim2, false, IfcSchema::IfcTrimmingPreference::IfcTrimmingPreference_PARAMETER); file.addEntity(trimmed_curve); IfcSchema::IfcCompositeCurveSegment* segment2 = new IfcSchema::IfcCompositeCurveSegment(IfcSchema::IfcTransitionCode::IfcTransitionCode_CONTSAMEGRADIENT, false, trimmed_curve); file.addEntity(segment2); segments->push(segment2); /*third segment - line */ IfcSchema::IfcCartesianPoint::list::ptr points2(new IfcSchema::IfcCartesianPoint::list()); points2->push(p4); points2->push(p5); file.addEntities(points2->generalize()); IfcSchema::IfcPolyline* poly2 = new IfcSchema::IfcPolyline(points2); file.addEntity(poly2); IfcSchema::IfcCompositeCurveSegment* segment3 = new IfcSchema::IfcCompositeCurveSegment(IfcSchema::IfcTransitionCode::IfcTransitionCode_CONTINUOUS, true, poly2); file.addEntity(segment3); segments->push(segment3); IfcSchema::IfcCompositeCurve* curve = new IfcSchema::IfcCompositeCurve(segments, false); file.addEntity(curve); IfcSchema::IfcSweptDiskSolid* solid = new IfcSchema::IfcSweptDiskSolid(curve, dia / 2, null, 0, 1); IfcSchema::IfcRepresentation::list::ptr reps(new IfcSchema::IfcRepresentation::list()); IfcSchema::IfcRepresentationItem::list::ptr items(new IfcSchema::IfcRepresentationItem::list()); items->push(solid); IfcSchema::IfcShapeRepresentation* rep = new IfcSchema::IfcShapeRepresentation( file.getSingle<IfcSchema::IfcRepresentationContext>(), S("Body"), S("AdvancedSweptSolid"), items); reps->push(rep); IfcSchema::IfcProductDefinitionShape* shape = new IfcSchema::IfcProductDefinitionShape(null, null, reps); file.addEntity(shape); rebar->setRepresentation(shape); IfcSchema::IfcObjectPlacement* storey_placement = file.getSingle<IfcSchema::IfcBuildingStorey>()->ObjectPlacement(); rebar->setObjectPlacement(file.addLocalPlacement(storey_placement, 0, 0, 0)); }
// Linked list chromosomes crossover void GaListMultipointCrossover::operator ()(GaCrossoverBuffer& crossoverBuffer, const GaCrossoverParams& parameters) const { // get chromosomes' representations Common::Data::GaListBase* source1 = &( (Representation::GaListStructureChromosome&)*crossoverBuffer.GetParentChromosome( 0 ) ).GetStructure(); Common::Data::GaListBase* source2 = &( (Representation::GaListStructureChromosome&)*crossoverBuffer.GetParentChromosome( 1 ) ).GetStructure(); // reserve memory for storing crossover points int maxCount = ( (const GaCrossoverPointParams&)parameters ).GetNumberOfCrossoverPoints() + 1; Common::Memory::GaAutoPtr<int> points1( new int[ maxCount ], Common::Memory::GaArrayDeletionPolicy<int>::GetInstance() ); Common::Memory::GaAutoPtr<int> points2( new int[ maxCount ], Common::Memory::GaArrayDeletionPolicy<int>::GetInstance() ); // create required number of offspring chromosomes for( int i = ( (const GaCrossoverPointParams&)parameters ).GetNumberOfOffspring() - 1; i >= 0; i -= 2 ) { // create the first offspring chromosome GaChromosomePtr offspring2, offspring1 = crossoverBuffer.CreateOffspringFromPrototype(); Common::Data::GaListBase* destination1 = &( (Representation::GaListStructureChromosome&)*offspring1 ).GetStructure(); // create the second offspring chromosome if required Common::Data::GaListBase* destination2 = NULL; if( i > 0 ) { offspring2 = crossoverBuffer.CreateOffspringFromPrototype(); destination2 = &( (Representation::GaListStructureChromosome&)*offspring2 ).GetStructure(); } // number of crossover points cannot be larger then smallest chromosome int count = maxCount; if( count > source1->GetCount() ) count = source1->GetCount(); if( count > source2->GetCount() ) count = source2->GetCount(); // generate crossover points if( count > 1 ) { Common::Random::GaGenerateRandomSequenceAsc( 1, source1->GetCount() - 1, count - 1, true, points1.GetRawPtr() ); Common::Random::GaGenerateRandomSequenceAsc( 1, source2->GetCount() - 1, count - 1, true, points2.GetRawPtr() ); } Common::Data::GaListNodeBase* sourceNode1 = source1->GetHead(); Common::Data::GaListNodeBase* sourceNode2 = source2->GetHead(); points1[ count - 1 ] = source1->GetCount(); points2[ count - 1 ] = source2->GetCount(); // alternately copy genes from parents to offspring chromosomes for( int j = 0, s1 = 0, s2 = 0; j < count ; j++ ) { int e1 = points1[ j ]; int e2 = points2[ j ]; // copy portion of the first parent to destination offspring if it exists if( destination1 ) { for( int k = s1; k < e1; k++, sourceNode1 = sourceNode1->GetNext() ) destination1->InsertTail( (Common::Data::GaListNodeBase*)sourceNode1->Clone() ); } // copy portion of the second parent to destination offspring if it exists if( destination2 ) { for( int k = s2; k < e2; k++, sourceNode2 = sourceNode2->GetNext() ) destination2->InsertTail( (Common::Data::GaListNodeBase*)sourceNode2->Clone() ); } // swap destination chromosomes Common::Data::GaListBase* t = destination1; destination1 = destination2; destination2 = t; s1 = e1; s2 = e2; } if( destination1->GetCount() == 0 || destination2->GetCount() == 0 ) { destination1 = destination1; } // store the first offspring crossoverBuffer.StoreOffspringChromosome( offspring1, 0 ); // store the second offspring if it was created if( !offspring2.IsNull() ) crossoverBuffer.StoreOffspringChromosome( offspring2, 1 ); } }
int main(int argc, const char* argv[]) { std::string opt_ip = "198.18.0.1";; std::string opt_data_folder = std::string(ROMEOTK_DATA_FOLDER); bool opt_Reye = false; // Learning folder in /tmp/$USERNAME std::string username; // Get the user login name vpIoTools::getUserName(username); // Create a log filename to save new files... std::string learning_folder; #if defined(_WIN32) learning_folder ="C:/temp/" + username; #else learning_folder ="/tmp/" + username; #endif // Test if the output path exist. If no try to create it if (vpIoTools::checkDirectory(learning_folder) == false) { try { // Create the dirname vpIoTools::makeDirectory(learning_folder); } catch (vpException &e) { std::cout << "Cannot create " << learning_folder << std::endl; std::cout << "Error: " << e.getMessage() <<std::endl; return 0; } } for (unsigned int i=0; i<argc; i++) { if (std::string(argv[i]) == "--ip") opt_ip = argv[i+1]; else if ( std::string(argv[i]) == "--reye") opt_Reye = true; else if (std::string(argv[i]) == "--help") { std::cout << "Usage: " << argv[0] << "[--ip <robot address>]" << std::endl; return 0; } } std::vector<std::string> chain_name(2); // LArm or RArm chain_name[0] = "LArm"; chain_name[1] = "RArm"; /************************************************************************************************/ /** Open the grabber for the acquisition of the images from the robot*/ vpNaoqiGrabber g; g.setFramerate(15); // Initialize constant transformations vpHomogeneousMatrix eMc; if (opt_Reye) { std::cout << "Using camera Eye Right" << std::endl; g.setCamera(3); // CameraRightEye eMc = g.get_eMc(vpCameraParameters::perspectiveProjWithDistortion,"CameraRightEye"); } else { std::cout << "Using camera Eye Right" << std::endl; g.setCamera(2); // CameraLeftEye eMc = g.get_eMc(vpCameraParameters::perspectiveProjWithDistortion,"CameraLeftEye"); } std::cout << "eMc: " << eMc << std::endl; if (! opt_ip.empty()) g.setRobotIp(opt_ip); g.open(); std::string camera_name = g.getCameraName(); std::cout << "Camera name: " << camera_name << std::endl; vpCameraParameters cam = g.getCameraParameters(vpCameraParameters::perspectiveProjWithDistortion); std::cout << "Camera parameters: " << cam << std::endl; /** Initialization Visp Image, display and camera paramenters*/ vpImage<unsigned char> I(g.getHeight(), g.getWidth()); vpDisplayX d(I); vpDisplay::setTitle(I, "Camera view"); //Initialize opencv color image cv::Mat cvI = cv::Mat(cv::Size(g.getWidth(), g.getHeight()), CV_8UC3); /************************************************************************************************/ /** Initialization target box*/ std::string opt_name_file_color_box_target = opt_data_folder + "/" +"target/plate_blob/color.txt"; vpBlobsTargetTracker box_tracker; bool status_box_tracker; vpHomogeneousMatrix cMbox; const double L = 0.04/2; std::vector <vpPoint> points(4); points[2].setWorldCoordinates(-L,-L, 0) ; points[1].setWorldCoordinates(-L,L, 0) ; points[0].setWorldCoordinates(L,L, 0) ; points[3].setWorldCoordinates(L,-L,0) ; box_tracker.setName("plate"); box_tracker.setCameraParameters(cam); box_tracker.setPoints(points); box_tracker.setGrayLevelMaxBlob(60); box_tracker.setLeftHandTarget(false); if(!box_tracker.loadHSV(opt_name_file_color_box_target)) { std::cout << "Error opening the file "<< opt_name_file_color_box_target << std::endl; } /************************************************************************************************/ /** Initialization target hands*/ std::string opt_name_file_color_target_path = opt_data_folder + "/" +"target/"; std::string opt_name_file_color_target_l = opt_name_file_color_target_path + chain_name[0] +"/color.txt"; std::string opt_name_file_color_target_r = opt_name_file_color_target_path + chain_name[1] +"/color.txt"; std::string opt_name_file_color_target1_l = opt_name_file_color_target_path + chain_name[0] +"/color1.txt"; std::string opt_name_file_color_target1_r = opt_name_file_color_target_path + chain_name[1] +"/color1.txt"; std::vector<vpBlobsTargetTracker*> hand_tracker; std::vector<bool> status_hand_tracker(2); std::vector<vpHomogeneousMatrix> cMo_hand(2); const double L1 = 0.025/2; std::vector <vpPoint> points1(4); points1[2].setWorldCoordinates(-L1,-L1, 0) ; points1[1].setWorldCoordinates(-L1,L1, 0) ; points1[0].setWorldCoordinates(L1,L1, 0) ; points1[3].setWorldCoordinates(L1,-L1,0) ; vpBlobsTargetTracker hand_tracker_l; hand_tracker_l.setName(chain_name[0]); hand_tracker_l.setCameraParameters(cam); hand_tracker_l.setPoints(points1); hand_tracker_l.setLeftHandTarget(true); if(!hand_tracker_l.loadHSV(opt_name_file_color_target_l)) std::cout << "Error opening the file "<< opt_name_file_color_target_l << std::endl; vpBlobsTargetTracker hand_tracker_r; hand_tracker_r.setName(chain_name[1]); hand_tracker_r.setCameraParameters(cam); hand_tracker_r.setPoints(points); hand_tracker_r.setLeftHandTarget(false); if(!hand_tracker_r.loadHSV(opt_name_file_color_target1_r)) std::cout << "Error opening the file "<< opt_name_file_color_target_r << std::endl; hand_tracker.push_back(&hand_tracker_l); hand_tracker.push_back(&hand_tracker_r); /************************************************************************************************/ // Constant transformation Target Frame to Arm end-effector (WristPitch) std::vector<vpHomogeneousMatrix> hand_Me_Arm(2); std::string filename_transform = std::string(ROMEOTK_DATA_FOLDER) + "/transformation.xml"; // Create twist matrix from box to Arm end-effector (WristPitch) std::vector <vpVelocityTwistMatrix> box_Ve_Arm(2); // Create twist matrix from target Frame to Arm end-effector (WristPitch) std::vector <vpVelocityTwistMatrix> hand_Ve_Arm(2); // Constant transformation Target Frame to box to target Hand std::vector<vpHomogeneousMatrix> box_Mhand(2); for (unsigned int i = 0; i < 2; i++) { std::string name_transform = "qrcode_M_e_" + chain_name[i]; vpXmlParserHomogeneousMatrix pm; // Create a XML parser if (pm.parse(hand_Me_Arm[i], filename_transform, name_transform) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK) { std::cout << "Cannot found the homogeneous matrix named " << name_transform << "." << std::endl; return 0; } else std::cout << "Homogeneous matrix " << name_transform <<": " << std::endl << hand_Me_Arm[i] << std::endl; hand_Ve_Arm[i].buildFrom(hand_Me_Arm[i]); } /************************************************************************************************/ /** Create a new istance NaoqiRobot*/ vpNaoqiRobot robot; if (! opt_ip.empty()) robot.setRobotIp(opt_ip); robot.open(); std::vector<std::string> jointNames = robot.getBodyNames("Head"); //jointNames.pop_back(); // We don't consider the last joint of the head = HeadRoll std::vector<std::string> jointNamesLEye = robot.getBodyNames("LEye"); std::vector<std::string> jointNamesREye = robot.getBodyNames("REye"); jointNames.insert(jointNames.end(), jointNamesLEye.begin(), jointNamesLEye.end()); std::vector<std::string> jointHeadNames_tot = jointNames; jointHeadNames_tot.push_back(jointNamesREye.at(0)); jointHeadNames_tot.push_back(jointNamesREye.at(1)); /************************************************************************************************/ std::vector<bool> grasp_servo_converged(2); grasp_servo_converged[0]= false; grasp_servo_converged[1]= false; std::vector<vpMatrix> eJe(2); // Initialize arms servoing vpServoArm servo_larm_l; vpServoArm servo_larm_r; std::vector<vpServoArm*> servo_larm; servo_larm.push_back(&servo_larm_l); servo_larm.push_back(&servo_larm_r); std::vector < std::vector<std::string > > jointNames_arm(2); jointNames_arm[0] = robot.getBodyNames(chain_name[0]); jointNames_arm[1] = robot.getBodyNames(chain_name[1]); // Delete last joint Hand, that we don't consider in the servo jointNames_arm[0].pop_back(); jointNames_arm[1].pop_back(); std::vector<std::string> jointArmsNames_tot = jointNames_arm[0]; jointArmsNames_tot.insert(jointArmsNames_tot.end(), jointNames_arm[1].begin(), jointNames_arm[1].end()); const unsigned int numArmJoints = jointNames_arm[0].size(); std::vector<vpHomogeneousMatrix> box_dMbox(2); // Vector containing the joint velocity vectors of the arms std::vector<vpColVector> q_dot_arm; // Vector containing the joint velocity vectors of the arms for the secondary task std::vector<vpColVector> q_dot_arm_sec; // Vector joint position of the arms std::vector<vpColVector> q; // Vector joint real velocities of the arms std::vector<vpColVector> q_dot_real; vpColVector q_temp(numArmJoints); q_dot_arm.push_back(q_temp); q_dot_arm.push_back(q_temp); q_dot_arm_sec.push_back(q_temp); q_dot_arm_sec.push_back(q_temp); q.push_back(q_temp); q.push_back(q_temp); q_dot_real.push_back(q_temp); q_dot_real.push_back(q_temp); // Initialize the joint avoidance scheme from the joint limits std::vector<vpColVector> jointMin; std::vector<vpColVector> jointMax; jointMin.push_back(q_temp); jointMin.push_back(q_temp); jointMax.push_back(q_temp); jointMax.push_back(q_temp); for (unsigned int i = 0; i< 2; i++) { jointMin[i] = robot.getJointMin(chain_name[i]); jointMax[i] = robot.getJointMax(chain_name[i]); jointMin[i].resize(numArmJoints,false); jointMax[i].resize(numArmJoints,false); // std::cout << jointMin[i].size() << std::endl; // std::cout << jointMax[i].size() << std::endl; // std::cout << "max " << jointMax[i] << std::endl; } std::vector<bool> first_time_arm_servo(2); first_time_arm_servo[0] = true; first_time_arm_servo[1] = true; std::vector< double> servo_arm_time_init(2); servo_arm_time_init[0] = 0.0; servo_arm_time_init[1] = 0.0; std::vector<int> cpt_iter_servo_grasp(2); cpt_iter_servo_grasp[0] = 0; cpt_iter_servo_grasp[1] = 0; unsigned int index_hand = 1; // vpHomogeneousMatrix M_offset; // M_offset[1][3] = 0.00; // M_offset[0][3] = 0.00; // M_offset[2][3] = 0.00; vpHomogeneousMatrix M_offset; M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ; double d_t = 0.01; double d_r = 0.0; bool first_time_box_pose = true; /************************************************************************************************/ vpMouseButton::vpMouseButtonType button; vpHomogeneousMatrix elMb; // Homogeneous matrix from right wrist roll to box vpHomogeneousMatrix cMbox_d; // Desired box final pose State_t state; state = CalibrateRigthArm; //AL::ALValue names_head = AL::ALValue::array("NeckYaw","NeckPitch","HeadPitch","HeadRoll","LEyeYaw", "LEyePitch","REyeYaw", "REyePitch" ); // Big Plate // AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(-23.2), vpMath::rad(16.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , 0.0, 0.0, 0.0 ); // small plate AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(-15.2), vpMath::rad(17.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , vpMath::rad(9.8), 0.0, 0.0 ); float fractionMaxSpeed = 0.1f; robot.getProxy()->setAngles(jointHeadNames_tot, angles_head, fractionMaxSpeed); while(1) { double loop_time_start = vpTime::measureTimeMs(); g.acquire(cvI); vpImageConvert::convert(cvI, I); vpDisplay::display(I); bool click_done = vpDisplay::getClick(I, button, false); // if (state < VSBox) // { char key[10]; bool ret = vpDisplay::getKeyboardEvent(I, key, false); std::string s = key; if (ret) { if (s == "r") { M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ; d_t = 0.0; d_r = 0.2; std::cout << "Rotation mode. " << std::endl; } if (s == "t") { M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ; d_t = 0.01; d_r = 0.0; std::cout << "Translation mode. " << std::endl; } if (s == "h") { hand_tracker[index_hand]->setManualBlobInit(true); hand_tracker[index_hand]->setForceDetection(true); } else if ( s == "+") { unsigned int value = hand_tracker[index_hand]->getGrayLevelMaxBlob() +10; hand_tracker[index_hand]->setGrayLevelMaxBlob(value); std::cout << "Set to "<< value << "the value of " << std::endl; } else if (s == "-") { unsigned int value = hand_tracker[1]->getGrayLevelMaxBlob()-10; hand_tracker[index_hand]->setGrayLevelMaxBlob(value-10); std::cout << "Set to "<< value << " GrayLevelMaxBlob. " << std::endl; } // |x // z\ | // \ | // \|_____ y // else if (s == "4") //-y { M_offset.buildFrom(0.0, -d_t, 0.0, 0.0, -d_r, 0.0) ; } else if (s == "6") //+y { M_offset.buildFrom(0.0, d_t, 0.0, 0.0, d_r, 0.0) ; } else if (s == "8") //+x { M_offset.buildFrom(d_t, 0.0, 0.0, d_r, 0.0, 0.0) ; } else if (s == "2") //-x { M_offset.buildFrom(-d_t, 0.0, 0.0, -d_r, 0.0, 0.0) ; } else if (s == "7")//-z { M_offset.buildFrom(0.0, 0.0, -d_t, 0.0, 0.0, -d_r) ; } else if (s == "9") //+z { M_offset.buildFrom(0.0, 0.0, d_t, 0.0, 0.0, d_r) ; } cMbox_d = cMbox_d * M_offset; } if (state < WaitPreGrasp) { status_hand_tracker[index_hand] = hand_tracker[index_hand]->track(cvI,I); if (status_hand_tracker[index_hand] ) { // display the tracking results cMo_hand[index_hand] = hand_tracker[index_hand]->get_cMo(); printPose("cMo right arm: ", cMo_hand[index_hand]); // The qrcode frame is only displayed when PBVS is active or learning vpDisplay::displayFrame(I, cMo_hand[index_hand], cam, 0.04, vpColor::none, 3); } } // } status_box_tracker = box_tracker.track(cvI,I); if (status_box_tracker ) { // display the tracking results cMbox = box_tracker.get_cMo(); printPose("cMo box: ", cMbox); // The qrcode frame is only displayed when PBVS is active or learning vpDisplay::displayFrame(I, cMbox, cam, 0.04, vpColor::none, 1); // if (first_time_box_pose) // { // // Compute desired box position cMbox_d // cMbox_d = cMbox * M_offset; // first_time_box_pose = false; // } // vpDisplay::displayFrame(I, cMbox_d, cam, 0.04, vpColor::red, 1); } if (state == CalibrateRigthArm && status_box_tracker && status_hand_tracker[index_hand] ) { vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to calibrate right hand", vpColor::red); vpHomogeneousMatrix box_Me_Arm; // Homogeneous matrix from the box to the left wrist box_Mhand[index_hand] = cMbox.inverse() * cMo_hand[index_hand]; box_Me_Arm = box_Mhand[index_hand] * hand_Me_Arm[index_hand] ; // from box to WristPitch // vpDisplay::displayFrame(I, cMo_hand[index_hand] * (cMbox.inverse() * cMo_hand[index_hand]).inverse() , cam, 0.04, vpColor::green, 1); if (click_done && button == vpMouseButton::button1 ) { box_Ve_Arm[index_hand].buildFrom(box_Me_Arm); index_hand = 0; state = CalibrateLeftArm; // BIG plate //AL::ALValue names_head = AL::ALValue::array("NeckYaw","NeckPitch","HeadPitch","HeadRoll","LEyeYaw", "LEyePitch","LEyeYaw", "LEyePitch" ); // AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(8.7), vpMath::rad(16.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , 0.0, 0.0, 0.0 ); // Small Plate AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(2.4), vpMath::rad(17.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , vpMath::rad(9.8), 0.0, 0.0 ); float fractionMaxSpeed = 0.1f; robot.getProxy()->setAngles(jointHeadNames_tot, angles_head, fractionMaxSpeed); click_done = false; } } if (state == CalibrateLeftArm && status_box_tracker && status_hand_tracker[index_hand] ) { vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to calibrate left hand", vpColor::red); vpHomogeneousMatrix box_Me_Arm; // Homogeneous matrix from the box to the left wrist box_Mhand[index_hand] = cMbox.inverse() * cMo_hand[index_hand]; box_Me_Arm = box_Mhand[index_hand] * hand_Me_Arm[index_hand] ; // from box to WristPitch // if (first_time_box_pose) // { // // Compute desired box position cMbox_d // cMbox_d = cMbox * M_offset; // first_time_box_pose = false; // } // vpDisplay::displayFrame(I, cMbox_d, cam, 0.04, vpColor::red, 1); // vpDisplay::displayFrame(I, cMo_hand[index_hand] * (cMbox.inverse() * cMo_hand[index_hand]).inverse() , cam, 0.04, vpColor::green, 1); if (click_done && button == vpMouseButton::button1 ) { box_Ve_Arm[index_hand].buildFrom(box_Me_Arm); state = WaitPreGrasp; click_done = false; //AL::ALValue names_head = AL::ALValue::array("NeckYaw","NeckPitch","HeadPitch","HeadRoll","LEyeYaw", "LEyePitch","LEyeYaw", "LEyePitch" ); AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(-6.8), vpMath::rad(16.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , 0.0, 0.0, 0.0 ); float fractionMaxSpeed = 0.05f; robot.getProxy()->setAngles(jointHeadNames_tot, angles_head, fractionMaxSpeed); } } if (state == WaitPreGrasp ) { index_hand = 1; if (click_done && button == vpMouseButton::button1 ) { state = PreGraps; click_done = false; } } if (state == PreGraps && status_box_tracker ) { vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to start the servo", vpColor::red); if (first_time_box_pose) { // Compute desired box position cMbox_d cMbox_d = cMbox * M_offset; first_time_box_pose = false; } vpDisplay::displayFrame(I, cMbox_d , cam, 0.05, vpColor::none, 3); vpDisplay::displayFrame(I, cMbox *box_Mhand[1] , cam, 0.05, vpColor::none, 3); vpDisplay::displayFrame(I, cMbox *box_Mhand[0] , cam, 0.05, vpColor::none, 3); if (click_done && button == vpMouseButton::button1 ) { state = VSBox; click_done = false; hand_tracker[index_hand] = &box_tracker; // Trick to use keys } } if (state == VSBox) { //Get Actual position of the arm joints q[0] = robot.getPosition(jointNames_arm[0]); q[1] = robot.getPosition(jointNames_arm[1]); // if(status_box_tracker && status_hand_tracker[index_hand] ) if(status_box_tracker ) { if (! grasp_servo_converged[0]) { // for (unsigned int i = 0; i<2; i++) // { unsigned int i = 0; //vpAdaptiveGain lambda(0.8, 0.05, 8); vpAdaptiveGain lambda(0.4, 0.02, 4); //servo_larm[i]->setLambda(lambda); servo_larm[i]->setLambda(0.2); eJe[i] = robot.get_eJe(chain_name[i]); servo_larm[i]->set_eJe(eJe[i]); servo_larm[i]->m_task.set_cVe(box_Ve_Arm[i]); box_dMbox[i] = cMbox_d.inverse() * cMbox; printPose("box_dMbox: ", box_dMbox[i]); servo_larm[i]->setCurrentFeature(box_dMbox[i]) ; vpDisplay::displayFrame(I, cMbox_d , cam, 0.05, vpColor::none, 3); // vpDisplay::displayFrame(I, cMbox *box_Mhand[1] , cam, 0.05, vpColor::none, 3); // vpDisplay::displayFrame(I, cMbox *box_Mhand[0] , cam, 0.05, vpColor::none, 3); if (first_time_arm_servo[i]) { std::cout << "-- Start visual servoing of the arm" << chain_name[i] << "." << std::endl; servo_arm_time_init[i] = vpTime::measureTimeSecond(); first_time_arm_servo[i] = false; } q_dot_arm[i] = - servo_larm[i]->computeControlLaw(servo_arm_time_init[i]); q_dot_real[0] = robot.getJointVelocity(jointNames_arm[0]); q_dot_real[1] = robot.getJointVelocity(jointNames_arm[1]); // std::cout << "real_q: " << std::endl << real_q << std::endl; // std::cout << " box_Ve_Arm[i]: " << std::endl << box_Ve_Arm[i] << std::endl; // std::cout << " eJe[i][i]: " << std::endl << eJe[i] << std::endl; //vpColVector real_v = (box_Ve_Arm[i] * eJe[i]) * q_dot_real[0]; vpColVector real_v = (box_Ve_Arm[i] * eJe[i]) * q_dot_arm[0]; // std::cout << "real_v: " << std::endl <<real_v << std::endl; // vpVelocityTwistMatrix cVo(cMo_hand); // vpMatrix cJe = cVo * oJo; // Compute the feed-forward terms // vpColVector sec_ter = 0.5 * ((servo_head.m_task_head.getTaskJacobianPseudoInverse() * (servo_head.m_task_head.getInteractionMatrix() * cJe)) * q_dot_larm); // std::cout <<"Second Term:" <<sec_ter << std::endl; //q_dot_head = q_dot_head + sec_ter; // Compute joint limit avoidance q_dot_arm_sec[0] = servo_larm[0]->m_task.secondaryTaskJointLimitAvoidance(q[0], q_dot_real[0], jointMin[0], jointMax[0]); //q_dot_arm_sec[1] = servo_larm[1]->m_task.secondaryTaskJointLimitAvoidance(q[1], q_dot_real[1], jointMin[1], jointMax[1]); // vpColVector q_dot_arm_head = q_dot_larm + q2_dot; //q_dot_arm_head.stack(q_dot_tot); // robot.setVelocity(joint_names_arm_head,q_dot_arm_head); //robot.setVelocity(jointNames_arm[i], q_dot_larm); // if (opt_plotter_arm) { // plotter_arm->plot(0, cpt_iter_servo_grasp, servo_larm.m_task.getError()); // plotter_arm->plot(1, cpt_iter_servo_grasp, q_dot_larm); // } // if (opt_plotter_q_sec_arm) // { // plotter_q_sec_arm->plot(0,loop_iter,q2_dot); // plotter_q_sec_arm->plot(1,loop_iter,q_dot_larm + q2_dot); // } cpt_iter_servo_grasp[i] ++; // } // // Visual servoing slave // i = 1; // //vpAdaptiveGain lambda(0.4, 0.02, 4); // servo_larm[i]->setLambda(0.07); // servo_larm[i]->set_eJe(robot.get_eJe(chain_name[i])); // servo_larm[i]->m_task.set_cVe(hand_Ve_Arm[i]); // box_dMbox[i] = (cMbox *box_Mhand[1]).inverse() * cMo_hand[1] ; // printPose("box_dMbox: ", box_dMbox[i]); // servo_larm[i]->setCurrentFeature(box_dMbox[i]) ; // vpDisplay::displayFrame(I, cMbox_d , cam, 0.025, vpColor::red, 2); // if (first_time_arm_servo[i]) { // std::cout << "-- Start visual servoing of the arm" << chain_name[i] << "." << std::endl; // servo_arm_time_init[i] = vpTime::measureTimeSecond(); // first_time_arm_servo[i] = false; // } // q_dot_arm[i] = - servo_larm[i]->computeControlLaw(servo_arm_time_init[i]); eJe[1] = robot.get_eJe(chain_name[1]); // q_dot_arm[1] += (box_Ve_Arm[1] * eJe[1]).pseudoInverse() * real_v; q_dot_arm[1] = (box_Ve_Arm[1] * eJe[1]).pseudoInverse() * real_v; vpColVector q_dot_tot = q_dot_arm[0] + q_dot_arm_sec[0]; q_dot_tot.stack( q_dot_arm[1] + q_dot_arm_sec[1]); robot.setVelocity(jointArmsNames_tot, q_dot_tot); vpTranslationVector t_error_grasp = box_dMbox[0].getTranslationVector(); vpRotationMatrix R_error_grasp; box_dMbox[0].extract(R_error_grasp); vpThetaUVector tu_error_grasp; tu_error_grasp.buildFrom(R_error_grasp); double theta_error_grasp; vpColVector u_error_grasp; tu_error_grasp.extract(theta_error_grasp, u_error_grasp); std::cout << "error: " << t_error_grasp << " " << vpMath::deg(theta_error_grasp) << std::endl; // if (cpt_iter_servo_grasp[0] > 100) { // vpDisplay::displayText(I, vpImagePoint(10,10), "Cannot converge. Click to continue", vpColor::red); // } // double error_t_treshold = 0.007; // if ( (sqrt(t_error_grasp.sumSquare()) < error_t_treshold) && (theta_error_grasp < vpMath::rad(3)) || (click_done && button == vpMouseButton::button1 /*&& cpt_iter_servo_grasp > 150*/) ) // { // robot.stop(jointArmsNames_tot); // state = End; // grasp_servo_converged[0] = true; // if (click_done && button == vpMouseButton::button1) // click_done = false; // } } } else { // state grasping but one of the tracker fails robot.stop(jointArmsNames_tot); } } if (state == End) { std::cout<< "End" << std::endl; } vpDisplay::flush(I) ; if (click_done && button == vpMouseButton::button3) { // Quit the loop break; } //std::cout << "Loop time: " << vpTime::measureTimeMs() - loop_time_start << std::endl; } robot.stop(jointArmsNames_tot); }