int mat2csv(std::string file_path, MatX data)
{
std::ofstream outfile(file_path);
// open file
if (outfile.good() != true) {
printf(E_CSV_DATA_OPEN, file_path.c_str());
return -1;
}
// save matrix
for (int i = 0; i < data.rows(); i++) {
for (int j = 0; j < data.cols(); j++) {
outfile << data(i, j);
if ((j + 1) != data.cols()) {
outfile << ",";
}
}
outfile << "\n";
}
// close file
outfile.close();
return 0;
}
void normalize_2dpts(double image_width, double image_height, MatX &pts)
{
Mat3 N;
MatX pts_h;
// convert points to homogeneous coordinates
pts_h.resize(pts.rows(), 3);
for (int i = 0; i < pts.rows(); i++) {
pts_h(i, 0) = pts(i, 0);
pts_h(i, 1) = pts(i, 1);
pts_h(i, 2) = 1.0;
}
// create normalization matrix N
N << 2.0 / image_width, 0.0, -1.0,
0.0, 2.0 / image_width, -1.0,
0.0, 0.0, 1.0;
// normalize image points to camera center
pts_h = (N * pts_h.transpose()).transpose();
// convert back to 2d coordinates
for (int i = 0; i < pts.rows(); i++) {
pts(i, 0) = pts_h(i, 0);
pts(i, 1) = pts_h(i, 1);
}
}
void projection_matrix(Mat3 K, Mat3 R, Vec3 t, MatX &P)
{
P.resize(3, 4);
P.block(0, 0, 3, 3) = R;
P.block(0, 3, 3, 1) = -(R * t);
P = K * P;
}
IGL_INLINE void igl::slice(
const MatX& X,
const Eigen::PlainObjectBase<DerivedR> & R,
const int dim,
MatY& Y)
{
Eigen::Matrix<typename DerivedR::Scalar,Eigen::Dynamic,1> C;
switch(dim)
{
case 1:
// boring base case
if(X.cols() == 0)
{
Y.resize(R.size(),0);
return;
}
igl::colon(0,X.cols()-1,C);
return slice(X,R,C,Y);
case 2:
// boring base case
if(X.rows() == 0)
{
Y.resize(0,R.size());
return;
}
igl::colon(0,X.rows()-1,C);
return slice(X,C,R,Y);
default:
assert(false && "Unsupported dimension");
return;
}
}
void mat2cvmat(MatX A, cv::Mat &B)
{
B = cv::Mat(A.rows(), A.cols(), CV_8UC1);
for (int i = 0; i < A.rows(); i++) {
for (int j = 0; j < A.cols(); j++) {
B.at<double>(i, j) = A(i, j);
}
}
}
void generateInitialTraj(int N, MatX& xi, MatX& q0, MatX& q1) {
xi.resize(N, 2);
q0.resize(1, 2);
q1.resize(1, 2);
q0 << -3, 5;
q1 << 5, -3;
for (int i=0; i<N; ++i) {
xi.row(i) = (i+1)*(q1-q0)/(N+1) + q0;
}
}
//Evaluate the constraints for Chompification
void TSRConstraint::evaluateConstraints(const MatX& qt,
MatX& h,
MatX& H)
{
//the pos is equivalent to the position of the
// end-effector in the robot frame.
double xyzrpy[6];
//the position of the ee.
Transform pos;
//get the position of the ee
forwardKinematics( qt, pos );
//get the position of the ee in the TSR frame
endeffectorToTSRFrame( pos, xyzrpy );
//the dimensionality of the configuration space
size_t DoF = qt.size() ;
//format h (constraint value vector).
if (size_t(h.rows()) != _dim_constraint || h.cols() != 1)
{
h.resize(_dim_constraint, 1);
}
int current_dim = 0;
std::vector< int > active_dims;
for ( int i = 0; i < _dim_constraint; i ++ )
{
const int dim = _dimension_id[i];
//if the robot's position goes over the TSR's upper bound:
if ( xyzrpy[dim] > _Bw(dim, 1) ){
h(current_dim) = xyzrpy[ dim ] - _Bw(dim, 1);
active_dims.push_back( dim );
current_dim ++;
}
//if the robot's position goes below the TSR's lower bound:
else if ( xyzrpy[dim] < _Bw( dim, 0 ) ){
h(current_dim) = xyzrpy[ dim ] - _Bw(dim, 0);
active_dims.push_back( dim );
current_dim ++;
}
}
if ( current_dim != _dim_constraint ){
h.conservativeResize( current_dim, 1 );
}
if ( H.rows() != current_dim || size_t( H.cols() ) != DoF ){
H.resize( current_dim, DoF );
}
computeJacobian( qt, pos, H, active_dims );
}
void generateInitialTraj(MotionOptimizer & chomper,
int N,
const vec2f& p0,
const vec2f& p1,
MatX& q0,
MatX& q1) {
q0.resize(1, 2);
q1.resize(1, 2);
q0 << p0.x(), p0.y();
q1 << p1.x(), p1.y();
chomper.getTrajectory().initialize( q0, q1, N );
}
void cvmat2mat(cv::Mat A, MatX &B)
{
B.resize(A.rows, A.cols);
for (int i = 0; i < A.rows; i++) {
for (int j = 0; j < A.cols; j++) {
B(i, j) = A.at<double>(i, j);
}
}
}
virtual double getCost(const MatX& q, size_t body_index,
MatX& dx_dq, MatX& cgrad )
{
assert( (q.rows() == 2 && q.cols() == 1) ||
(q.rows() == 1 && q.cols() == 2) );
dx_dq.resize(3, 2);
dx_dq.setZero();
dx_dq << 1, 0, 0, 1, 0, 0;
cgrad.resize(3, 1);
vec3f g;
float c = map->sampleCost(vec3f(q(0), q(1), 0.0), g);
cgrad << g[0], g[1], 0.0;
return c;
}
void cvpts2mat(std::vector<cv::Point2f> points, MatX &mat)
{
cv::Point2f p;
mat.resize(points.size(), 3);
for (int i = 0; i < points.size(); i++) {
p = points[i];
mat(i, 0) = p.x;
mat(i, 1) = p.y;
mat(i, 2) = 1.0;
}
}
int KalmanFilter::estimate(MatX A, VecX y)
{
// prediction update
mu_p = A * mu;
S_p = A * S * A.transpose() + R;
// measurement update
K = S_p * C.transpose() * (C * S_p * C.transpose() + Q).inverse();
mu = mu_p + K * (y - C * mu_p);
S = (I - K * C) * S_p;
return 0;
}
virtual void evaluateConstraints(const MatX& qt,
MatX& h,
MatX& H){
//bounds col0: lower bounds
// col1: upper bounds
// row0: x-values
// row1: y-values
MatX bounds(2,2);
bounds << 4, 6,
-4, -2;
h.resize(2,1);
H.resize(2,2);
H << 1, 0,
0, 1;
for ( int i = 0; i < 2; i ++ ){
if ( qt(i) < bounds(i,0)){ h(i) = qt(i) - bounds(i,0); }
else if ( qt(i) > bounds(i,1)){ h(i) = qt(i) - bounds(i,1); }
else { h(i) = 0; H(i,i) = 0.0; }
}
}
double ProblemDescription::evaluateConstraint( MatX & h, MatX & H )
{
if ( factory.empty() ) {return 0; }
TIMER_START( "constraint" );
prepareData();
h.resize( factory.numOutput(), 1 );
H.resize( size(), factory.numOutput() );
double magnitude = factory.evaluate( trajectory, h, H );
if ( doing_covariant ) {
metric.multiplyLowerInverse(
MatMap ( H.data(), trajectory.N(),
trajectory.M()*factory.numOutput() ) );
}
TIMER_STOP( "constraint" );
return magnitude;
}
double ProblemDescription::evaluateConstraint( MatX & h )
{
if ( factory.empty() ) {return 0; }
TIMER_START( "constraint" );
prepareData();
h.resize( factory.numOutput(), 1 );
const double value = factory.evaluate( trajectory, h);
TIMER_STOP( "constraint" );
return value;
}
int csv2mat(std::string file_path, bool header, MatX &data)
{
int line_no;
int nb_rows;
int nb_cols;
std::string line;
std::ifstream infile(file_path);
std::vector<double> vdata;
std::string element;
double value;
// load file
if (infile.good() != true) {
printf(E_CSV_DATA_LOAD, file_path.c_str());
return -1;
}
// obtain number of rows and cols
nb_rows = csvrows(file_path);
nb_cols = csvcols(file_path);
// header line?
if (header) {
std::getline(infile, line);
nb_rows -= 1;
}
// load data
line_no = 0;
data.resize(nb_rows, nb_cols);
while (std::getline(infile, line)) {
std::istringstream ss(line);
// load data row
for (int i = 0; i < nb_cols; i++) {
std::getline(ss, element, ',');
value = atof(element.c_str());
data(line_no, i) = value;
}
line_no++;
}
return 0;
}
virtual void evaluateConstraints(const MatX& qt,
MatX& h,
MatX& H){
assert((qt.cols()==1 && qt.rows()==2) || (qt.cols() == 2 && qt.rows() == 1));
h.conservativeResize(1,1);
H.conservativeResize(1,2);
h(0) = mydot(qt,qt) - 4;
for(int i=0; i<2; i++){
H(i) = 2*qt(i);
}
}
void clear() {
A.empty();
b.empty();
}
//-------------------------
// Methods
//-------------------------
void resize(int_8 const &a) {
N = a;
A.resize(a);
b.resize(a);
}
void ConstantConstraint::evaluateConstraints(const MatX& qt,
MatX& h,
MatX& H)
{
assert(qt.rows() == 1 || qt.cols() == 1);
size_t DoF = std::max(qt.rows(), qt.cols());
if (size_t(h.rows()) != numCons || h.cols() != 1) {
h.resize(numCons, 1);
}
if (size_t(H.rows()) != numCons || size_t(H.cols()) != DoF) {
H.resize(numCons, DoF);
}
H.setZero();
for(size_t i=0; i<numCons; i++){
assert(index[i] < DoF);
h(i) = qt(index[i])-value[i];
H(i,index[i]) = 1;
}
}
