optimizer_gnuplot.cpp

Go to the documentation of this file.

#include <cstdio>
#include <iostream>
#include <cstring>
#include <fstream>  
#include <optimizer_gnuplot.h>

using namespace robotLibPbD;

 
bool COptimizerGnuplot::generateGnuPlotCoords(double *st, std::string filenameScript, std::string filenameData)
{
  setData(examples[0]);
  setValue(st);
  std::string text;
  
  CMatrix pose;
  std::vector<CVec> points;
  const double len = 20.0;
  points.push_back(CVec(0.0, 0.0, 0.0));
  points.push_back(CVec(len, 0.0, 0.0));
  points.push_back(CVec(0.0, 0.0, 0.0));
  points.push_back(CVec(0.0, len, 0.0));
  points.push_back(CVec(0.0, 0.0, 0.0));
  points.push_back(CVec(0.0, 0.0, len));
  
  std::vector<int> linestyle;
  linestyle.push_back(1);
  linestyle.push_back(1);
  linestyle.push_back(2);
  linestyle.push_back(2);
  linestyle.push_back(3);
  linestyle.push_back(3);
  
  std::string plotdata;
  for (unsigned int i=0; i<frames.getFrames().size(); i++)
    {
      pose = frames.getFrame(i)->getRelativeToBase();
      for (unsigned int j=0; j<points.size(); j++)
        plotdata += (pose * points[j]).toString() + "\n";
    }
  
  std::ofstream textstream2(filenameData.c_str());
  if (textstream2.fail())
    return false;
  
  textstream2 << plotdata;
  textstream2.close();
  
  std::vector<double> mins(3, 1.0e30);
  std::vector<double> maxs(3, -1.0e30);
  for (unsigned int i=0; i<frames.getFrames().size(); i++)
    {
      pose = frames.getFrame(i)->getRelativeToBase();
      for (unsigned int k=0; k<3; k++)
        if (pose.a[12 + k] < mins[k])
          mins[k] = pose.a[12 + k];
        else if (pose.a[12 + k] > maxs[k])
          maxs[k] = pose.a[12 + k];
      
      for (unsigned int j=0; j<points.size(); j+=2)
        {
          CVec t1, t2;
          t1 = pose * points[j];
          t2 = pose * points[j+1]; 
          text += printToString("set arrow from %f,%f,%f to %f,%f,%f linestyle %d\n",
                                t1.x, t1.y, t1.z,
                                t2.x, t2.y, t2.z,
                                linestyle[i]);
          
        }
    }
  
  text += "splot 'plot_data' with points ps 0\n";
  text += "pause -1\n";
  
  
  std::ofstream textstream(filenameScript.c_str());
  if (textstream.fail())
    return false;
  
  textstream << text;
  textstream.close();
  return true;
}

bool COptimizerGnuplot::generateGnuPlot(double *st, std::string filenameScript, std::string filenameData)
{
  std::string text;
  std::string plotdata; 
  CVec t1, t2;
  std::vector<CVec> t1start(functions.size());
  for (unsigned int i=0; i<examples.size(); i++)
    { 
      setData(examples[i]);
      setValue(st);
      for (unsigned int j=0; j<functions.size(); j++)
        {
          functions[j]->getDistance(t1, t2);
 
          if (i == 0)
            t1start[j] = t1;

          text += printToString("set arrow from %f,%f,%f to %f,%f,%f linestyle %d\n",
                                t1.x, t1.y, t1.z,
                                t2.x, t2.y, t2.z,
                                1);

          plotdata += printToString("%f %f %f\n",
                                t1.x, t1.y, t1.z);

          functions[j]->getDistance(t2);
          text += printToString("set arrow from %f,%f,%f to %f,%f,%f linestyle %d\n",
                                t1start[j].x, t1start[j].y, t1start[j].z,
                                t1start[j].x + t2.x, t1start[j].y + t2.y, t1start[j].z + t2.z,
                                3);
        } 
    }
   
  text += "splot 'plot_data' with points ps 0\n";
  text += "pause -1\n"; 

  std::ofstream textstream(filenameScript.c_str());
  if (textstream.fail())
    return false;
  
  textstream << text;
  textstream.close();
  
  std::ofstream textstream2(filenameData.c_str());
  if (textstream2.fail())
    return false;
  
  textstream2 << plotdata;
  textstream2.close();
  return true;
}