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