int main (int argc, char *argv[]) {
if (argc < 3) {
std::cerr << "Usage: " << argv[0]
<< " deploy.prototxt network.caffemodel"
<< " [gpu_id]" << std::endl;
return 1;
}
::google::InitGoogleLogging(argv[0]);
const string& model_file = argv[1];
const string& trained_file = argv[2];
int gpu_id = 0;
if (argc >= 4) {
gpu_id = atoi(argv[3]);
}
const bool do_train = false;
Regressor regressor(model_file, trained_file, gpu_id, do_train);
// Ensuring randomness for fairness.
srandom(time(NULL));
// Create a tracker object.
const bool show_intermediate_output = false;
Tracker tracker(show_intermediate_output);
VOT vot; // Initialize the communcation
// Get region and first frame
VOTRegion region = vot.region();
string path = vot.frame();
// Load the first frame and use the initialization region to initialize the tracker.
tracker.Init(path, region, ®ressor);
//track
while (true) {
path = vot.frame(); // Get the next frame
if (path.empty()) break; // Are we done?
// Load current image.
const cv::Mat& image = cv::imread(path);
// Track and estimate the bounding box location.
BoundingBox bbox_estimate;
tracker.Track(image, ®ressor, &bbox_estimate);
bbox_estimate.GetRegion(®ion);
vot.report(region); // Report the position of the tracker
}
// Finishing the communication is completed automatically with the destruction
// of the communication object (if you are using pointers you have to explicitly
// delete the object).
return 0;
}
double SoftmaxRegression<OptimizerType>::Train(const arma::mat& data,
const arma::Row<size_t>& labels,
const size_t numClasses)
{
SoftmaxRegressionFunction regressor(data, labels, numClasses,
lambda, fitIntercept);
OptimizerType<SoftmaxRegressionFunction> optimizer(regressor);
return Train(optimizer);
}
int calculateEssentialParametersSubspace(DynamicRegressorGenerator & regressor_generator,
const IBatchDynamicDataset & dataset,
Eigen::MatrixXd & essential_parameters_subspace,
Eigen::VectorXd & /*sigma*/,
const double tol
)
{
if( regressor_generator.getNrOfParameters() <= 0 ) { std::cerr << "calculateEssentialParametersSubspace error: regressor_generator not consistent" << std::endl; return -2; }
//Y^T*Y nrOfParams x nfOfParams matrix encoding the essential parameters subspace
Eigen::MatrixXd YTY(regressor_generator.getNrOfParameters(),regressor_generator.getNrOfParameters());
Eigen::MatrixXd regressor(regressor_generator.getNrOfOutputs(),regressor_generator.getNrOfParameters());
Eigen::VectorXd known_terms(regressor_generator.getNrOfOutputs());
DynamicSample sample;
//Initial value is zero
YTY.setZero();
for(int i=0; i < dataset.getNrOfSamples(); i++ ) {
bool status = dataset.getSample(i,sample);
if( !status ) { return -3; }
if( regressor_generator.getNrOfDOFs() != sample.getNrOfDOFs() ) {
std::cerr << "calculateEssentialParametersSubspace error: mismatch between regressor generator ( "
<< regressor_generator.getNrOfDOFs() << " dofs ) and data sample (" << sample.getNrOfDOFs() << " dofs ) " <<std::endl;
return -1;
}
//Set robot state and sensors
regressor_generator.setRobotStateAndSensors(sample);
//Get regressor
regressor_generator.computeRegressor(regressor,known_terms);
YTY += regressor.transpose()*regressor;
}
int status = getRowSpaceBasis(YTY,essential_parameters_subspace,tol,true);
if( status != 0 ) { std::cerr << "calculateEssentialParametersSubspace error: getRowSpaceBasis failed" << std::endl; return -4; }
return 0;
}
SoftmaxRegression<OptimizerType>::SoftmaxRegression(const arma::mat& data,
const arma::Row<size_t>& labels,
const size_t numClasses,
const double lambda,
const bool fitIntercept) :
numClasses(numClasses),
lambda(lambda),
fitIntercept(fitIntercept)
{
SoftmaxRegressionFunction regressor(data, labels, numClasses,
lambda, fitIntercept);
OptimizerType<SoftmaxRegressionFunction> optimizer(regressor);
parameters = regressor.GetInitialPoint();
Train(optimizer);
}
int main()
{
std::string icub_urdf_filename = "icub_model.urdf";
// Create the iCub model
KDL::Tree icub_tree;
bool ok = iDynTree::treeFromUrdfFile(icub_urdf_filename,icub_tree);
if( !ok )
{
std::cerr <<"Could not import icub urdf" << std::endl;
return EXIT_FAILURE;
}
// Create the iCub undirected tree, that contains also
// a default serialization (i.e. a mapping between links/joints
// and integers (if you want to provide a user-defined serialization
// to the undirected tree, pass it as a second argument to the constructor
KDL::CoDyCo::UndirectedTree icub_undirected_tree(icub_tree);
std::cout << "icub_tree serialization 1 : " << icub_undirected_tree.getSerialization().toString();
// Load a sensors tree (for ft sensors) from the information extracted from urdf file
// and using the serialization provided in the undirected tree
iDynTree::SensorsList sensors_tree = iDynTree::sensorsListFromURDF(icub_undirected_tree,icub_urdf_filename);
//Create a regressor generator
KDL::CoDyCo::Regressors::DynamicRegressorGenerator regressor_generator(icub_undirected_tree,sensors_tree);
//Add a set of rows of the regressor of right arm
std::vector<std::string> l_arm_subtree;
l_arm_subtree.push_back("l_upper_arm");
regressor_generator.addSubtreeRegressorRows(l_arm_subtree);
//Create a random state for the robot
KDL::Twist base_velocity, base_acceleration;
base_velocity = base_acceleration = KDL::Twist::Zero();
KDL::JntArray q(regressor_generator.getNrOfDOFs());
SetToZero(q);
srand(time(0));
for(int i=0; i < q.rows(); i++ )
{
q(i) = random_double();
}
double gravity_norm = 9.8;
base_acceleration.vel[2] = -gravity_norm;
regressor_generator.setRobotState(q,q,q,base_velocity,base_acceleration);
// Estimate sensor measurements from the model
iDynTree::Wrench simulate_measurement = simulateFTSensorFromKinematicState(icub_undirected_tree,
q,q,q,base_velocity,base_acceleration,"l_arm_ft_sensor",sensors_tree);
//Create a regressor and check the returned sensor value
Eigen::MatrixXd regressor(regressor_generator.getNrOfOutputs(),regressor_generator.getNrOfParameters());
Eigen::VectorXd kt(regressor_generator.getNrOfOutputs());
regressor_generator.computeRegressor(regressor,kt);
//std::cout << "regressors : " << regressor << std::endl;
Eigen::VectorXd parameters(regressor_generator.getNrOfParameters());
parameters.setZero();
regressor_generator.getModelParameters(parameters);
assert(parameters.rows() == regressor_generator.getNrOfParameters());
Eigen::Matrix<double,6,1> sens = regressor*parameters;
////////////////////////////////////////////////////////////
///// Test also the new iDynTree regressor infrastructure
////////////////////////////////////////////////////////////
iDynTree::Regressors::DynamicsRegressorGenerator new_generator;
// load robot and sensor model
ok = ok && new_generator.loadRobotAndSensorsModelFromFile(icub_urdf_filename);
assert(ok);
// load regressor structure (this should be actually loaded from file)
std::string regressor_structure
= "<regressor> "
" <subtreeBaseDynamics> "
" <FTSensorLink>l_upper_arm</FTSensorLink> "
" </subtreeBaseDynamics> "
"</regressor>";
ok = ok && new_generator.loadRegressorStructureFromString(regressor_structure);
assert(ok);
iDynTree::VectorDynSize q_idyntree(q.rows());
ok = ok && iDynTree::ToiDynTree(q,q_idyntree);
assert(ok);
iDynTree::Twist gravity;
gravity(2) = gravity_norm;
ok = ok && new_generator.setRobotState(q_idyntree,q_idyntree,q_idyntree,gravity);
assert(ok);
iDynTree::MatrixDynSize regr_idyntree(new_generator.getNrOfOutputs(),new_generator.getNrOfParameters());
iDynTree::VectorDynSize kt_idyntree(new_generator.getNrOfOutputs());
iDynTree::VectorDynSize param_idyntree(new_generator.getNrOfParameters());
ok = ok && new_generator.getModelParameters(param_idyntree);
int sensorIndex = new_generator.getSensorsModel().getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE,"l_arm_ft_sensor");
ok = ok && new_generator.getSensorsMeasurements().setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,sensorIndex,simulate_measurement);
ok = ok && new_generator.computeRegressor(regr_idyntree,kt_idyntree);
Eigen::Matrix<double,6,1> sens_idyntree = Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(regr_idyntree.data(),regr_idyntree.rows(),regr_idyntree.cols())*
Eigen::Map<Eigen::VectorXd>(param_idyntree.data(),param_idyntree.size());
Eigen::Matrix<double,6,1> kt_eigen = Eigen::Map< Eigen::Matrix<double,6,1> >(kt_idyntree.data(),kt_idyntree.size());
std::cout << "Parameters norm old " << parameters.norm() << std::endl;
std::cout << "Parameters norm new " << Eigen::Map<Eigen::VectorXd>(param_idyntree.data(),param_idyntree.size()).norm() << std::endl;
std::cout << "Sensor measurement from regressor*model_parameters: " << sens << std::endl;
std::cout << "Sensor measurement from regressor*model_parameters (new): " << sens_idyntree << std::endl;
std::cout << "Sensor measurement from RNEA: " << KDL::CoDyCo::toEigen( iDynTree::ToKDL(simulate_measurement)) << std::endl;
std::cout << "Sensor measurement from known terms (new) " << kt_eigen << std::endl;
double tol = 1e-5;
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+sens).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error" << std::endl;
return EXIT_FAILURE;
}
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+sens_idyntree).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error: failure in new iDynTree regressor generator" << std::endl;
return EXIT_FAILURE;
}
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+kt_eigen).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error: failure in new iDynTree regressor generator" << std::endl;
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
int
main(int argc, char *argv[])
{
std::string model_file = "tracker.prototxt";
std::string trained_file = "tracker.caffemodel";
int gpu_id = 0;
Regressor regressor(model_file, trained_file, gpu_id, false);
// Ensuring randomness for fairness.
srandom (time(NULL));
// Create a tracker object.
const bool show_intermediate_output = false;
Tracker tracker(show_intermediate_output);
//VOT vot; // Initialize the communcation
// Get region and first frame
//VOTRegion region = vot.region();
std::string path; // TODO // = vot.frame();
// Load the first frame and use the initialization region to initialize the tracker.
cv::Mat m = cv::imread("lena.jpg");
cv::Mat mv = cv::imread("lena.jpg");
BoundingBox b;
b.x1_ = 246;
b.y1_ = 250;
b.x2_ = 294;
b.y2_ = 285;
tracker.Init(m, b, ®ressor); // TODO
b.DrawBoundingBox(&mv);
// Track and estimate the bounding box location.
BoundingBox bbox_estimate = b;
double theta = 0;
int illumination_m = 1;
for (;;)
{
cv::Mat n = m.clone();
cv::Point center = cv::Point(n.cols / 2, n.rows / 2);
cv::Mat rot = cv::getRotationMatrix2D(center, theta, 1.0);
cv::warpAffine(m, n, rot, n.size());
if (illumination_m > 0)
n *= illumination_m;
else if (illumination_m < 0)
n /= (-illumination_m);
bbox_estimate.DrawBoundingBox(&n);
// cv::Mat mb(m, cv::Rect(b.x1_, b.y1_, b.x2_ - b.x1_, b.y2_ - b.y1_));
// cv::Mat nb(n, cv::Rect(bbox_estimate.x1_, bbox_estimate.y1_, bbox_estimate.x2_ - bbox_estimate.x1_, bbox_estimate.y2_ - bbox_estimate.y1_));
// double dcolor = color_based(&m, &n, &b, &bbox_estimate);
// double dstructure = structure_based(&m, &n, &b, &bbox_estimate);
// cv::imshow("mb", mb);
// cv::imshow("nb", nb);
// printf("COLOR: %lf STRUCTURE: %lf\n", dcolor, dstructure);
cv::imshow("m", mv);
cv::imshow("n", n);
char c = cv::waitKey(-1);
switch (c)
{
case 'w':
bbox_estimate.y1_ -= 2;
bbox_estimate.y2_ -= 2;
break;
case 's':
bbox_estimate.y1_ += 2;
bbox_estimate.y2_ += 2;
break;
case 'a':
bbox_estimate.x1_ -= 2;
bbox_estimate.x2_ -= 2;
break;
case 'd':
bbox_estimate.x1_ += 2;
bbox_estimate.x2_ += 2;
break;
case 't':
n = m.clone();
center = cv::Point(n.cols / 2, n.rows / 2);
rot = cv::getRotationMatrix2D(center, theta, 1.0);
cv::warpAffine(m, n, rot, n.size());
if (illumination_m > 0)
n *= illumination_m;
else if (illumination_m < 0)
n /= (-illumination_m);
tracker.Track(n, ®ressor, &bbox_estimate);
break;
case 'f':
theta += 2;
break;
case 'r':
theta -= 2;
break;
case 'y':
illumination_m += 2;
break;
case 'h':
illumination_m -= 2;
break;
case 'x':
bbox_estimate = b;
break;
case 'q':
return 0;
}
}
//vot.report(region); // Report the position of the tracker
// Finishing the communication is completed automatically with the destruction
// of the communication object (if you are using pointers you have to explicitly
// delete the object).
return 0;
}
int main(int argc, char *argv[])
{
// In this example we will use YARP only for parsing command line
// parameters
yarp::os::ResourceFinder rf;
rf.setVerbose(true);
rf.setDefault("name","walkmanLimbJTSRegressor");
rf.setDefault("config","config.ini");
rf.configure(argc,argv);
if (rf.check("help"))
{
yInfo() << "Options:\n\n";
yInfo() << "\t--urdf file: name of the URDF file containing"
" the model of the Walkman. \n";
return 0;
}
std::string urdf_filename = rf.findFile("urdf");
yInfo() << "Tryng to open " << urdf_filename << " as Walkman model";
/**
* The dynamics regressor generator is a class for calculating arbitrary regressor
* related to robot dynamics, for identification of dynamics parameters, such
* as inertial parameters (masses, centers of mass, inertia tensor elements).
*/
//For building the regressor generator, we need several inputs:
// * the model of the robot, given by the KDL::CoDyCo::UndirectedTree object
// * a model of the sensors of the robot, given by the KDL::CoDyCo::SensorsTree object
// (at this moment the SensorsTree supports only six-axis FT sensors, so it is not needed
// for joint torque sensors regressors)
// * a kinematic base (i.e. link for which the velocity/acceleration and the gravity are known:
// in this example we will leave the default one, i.e. the root of the robot)
// * the option for considering or not FT sensors offset in the parameter estimation
// (in this example we don't consider FT sensors, so we put this to false)
// * a list of "fake_links", for which no inertial parameters will be considered, useful for removing
// from the model "fake" links such as frames.
KDL::Tree walkman_tree;
if( !kdl_format_io::treeFromUrdfFile(urdf_filename,walkman_tree) )
{
std::cerr << " Could not parse urdf robot model" << std::endl;
return EXIT_FAILURE;
}
KDL::CoDyCo::UndirectedTree walkman_undirected_tree(walkman_tree);
KDL::CoDyCo::SensorsTree walkman_sensors_tree;
std::string kinematic_base = "";
bool consider_ft_offsets = false;
std::vector< std::string > fake_links;
KDL::CoDyCo::Regressors::DynamicRegressorGenerator jts_regressor_generator(walkman_undirected_tree,
walkman_sensors_tree,
kinematic_base,
consider_ft_offsets,
fake_links);
// We add the structure now: the list of joint torque sensors that we are considering
std::vector< std::string > jts_names;
for(int jts = 0; jts < jts_names.size(); jts++ )
{
jts_regressor_generator.addTorqueRegressorRows(jts_names[jts]);
}
// Now we can print some information about the regressor
std::cout << "Regressor information: " << std::endl;
std::cout << "Outputs: " << std::endl;
std::cout << jts_regressor_generator.getDescriptionOfOutputs() << std::endl;
std::cout << "Parameters: " << std::endl;
std::cout << jts_regressor_generator.getDescriptionOfParameters() << std::endl;
// We now have to set the robot state (in the fixed base case);
KDL::JntArray q(walkman_undirected_tree.getNrOfDOFs());
KDL::JntArray dq(walkman_undirected_tree.getNrOfDOFs());
KDL::JntArray ddq(walkman_undirected_tree.getNrOfDOFs());
// For this example we will consider all joint positions, velocities and accelerations to zero
KDL::SetToZero(q);
KDL::SetToZero(dq);
KDL::SetToZero(ddq);
KDL::Vector gravity;
gravity(2) = -9.8;
KDL::Twist gravity_twist(gravity,KDL::Vector::Zero());
jts_regressor_generator.setRobotState(q,dq,ddq,gravity_twist);
// Now we can generate the actual regressor
Eigen::MatrixXd regressor(jts_regressor_generator.getNrOfOutputs(),jts_regressor_generator.getNrOfParameters());
Eigen::VectorXd known_terms(jts_regressor_generator.getNrOfOutputs());
jts_regressor_generator.computeRegressor(regressor,known_terms);
return 0;
}
