// ------------------------------------------------------------------------
// Generate a MATLAB script that draws the overall navigation log
// ------------------------------------------------------------------------
void navlog_viewer_GUI_designDialog::OnmnuMatlabPlotsSelected(wxCommandEvent& event)
{
WX_START_TRY
wxFileDialog dialog(
this,
_("Save MATLAB/Octave script that draws the log...") /*caption*/,
_(".") /* defaultDir */,
_("drawLog.m") /* defaultFilename */,
_("MATLAB/Octave script (*.m)|*.m") /* wildcard */,
wxFD_SAVE | wxFD_OVERWRITE_PROMPT );
if (dialog.ShowModal() != wxID_OK) return;
const string filName(dialog.GetPath().mb_str());
ofstream f(filName.c_str());
if (!f.is_open())
throw runtime_error("Error writing to file!");
f << "% Script for drawing navigation log\n"
<< "% Generated automatically by navlog-viewer - MRPT " << mrpt::system::MRPT_getVersion() << "\n"
<< "% From log: " << string(edLogFile->GetValue().mbc_str()) << "\n"
<< "% -------------------------------------------------------------------------\n\n";
f << "%%\n"
<< "function [] = main()\n"
<< "% Robot shape: (x,y) in each line\n"
<< "rs = [-0.3 -0.3;0.6 -0.3;0.6 0.3;-0.3 0.3];\n"
<< "decimate_robot_shapes = 15;"
<< "decim_cnt=0;";
const int DECIMATE_POINTS = 10;
int decim_point_cnt =0;
std::vector<float> X,Y; // Obstacles
std::vector<float> TX,TY; // Target over time
const size_t N = m_logdata.size();
for (size_t i=0;i<N;i++)
{
const CLogFileRecordPtr logsptr = CLogFileRecordPtr( m_logdata[i] );
const CLogFileRecord * logptr = logsptr.pointer();
f << format("decim_cnt=decim_cnt+1; if (decim_cnt>=decimate_robot_shapes); drawRobotShape(rs,[%f %f %f]); decim_cnt=0; end\n",
logptr->robotOdometryPose.x(),
logptr->robotOdometryPose.y(),
logptr->robotOdometryPose.phi()
);
if (++decim_point_cnt>=DECIMATE_POINTS)
{
CSimplePointsMap pts;
pts.changeCoordinatesReference( logptr->WS_Obstacles, logptr->robotOdometryPose );
const std::vector<float> &pX = pts.getPointsBufferRef_x();
const std::vector<float> &pY = pts.getPointsBufferRef_y();
X.insert(X.begin(),pX.begin(),pX.end());
Y.insert(Y.begin(),pY.begin(),pY.end());
}
// Target:
const mrpt::math::TPoint2D trg_glob = mrpt::math::TPoint2D( logptr->robotOdometryPose+logptr->WS_target_relative );
if (TX.empty() || std::abs((*TX.rbegin())-trg_glob.x)>1e-3 || std::abs((*TY.rbegin())-trg_glob.y)>1e-3 )
{
TX.push_back(trg_glob.x);
TY.push_back(trg_glob.y);
}
}
f << "\n % Points: \n"
<< " Ps = [";
for (size_t k=0;k<X.size();k++)
f << X[k] << " " << Y[k] << "\n";
f << "];\n"
<< "plot(Ps(:,1),Ps(:,2),'k.','MarkerSize',3);\n";
f << "\n % Target point: \n"
<< " Ts = [";
for (size_t k=0;k<TX.size();k++)
f << TX[k] << " " << TY[k] << "\n";
f << "];\n"
<< "plot(Ts(:,1),Ts(:,2),'rx','MarkerSize',5);\n";
f << "axis equal;\n"
<< "\n";
f
<< "%% drawRobotShape()\n"
<< "function [] = drawRobotShape(sh,pose)\n"
<< "nPts=size(sh,1);\n"
<< "Pts=[];\n"
<< "for i=1:(nPts+1),\n"
<< " j=mod(i-1,nPts)+1;\n"
<< " cc=cos(pose(3)); ss=sin(pose(3)); x=pose(1); y=pose(2);\n"
<< " Pts=[Pts;x+cc*sh(j,1)-ss*sh(j,2) y+ss*sh(j,1)+cc*sh(j,2) ];\n"
<< "end\n"
<< "plot(Pts(:,1),Pts(:,2)); hold on;\n";
WX_END_TRY
}
uint32_t CSimplePointsMap_getSize(CSimplePointsMap& self)
{
return self.getPointsBufferRef_x().size();
}
