void Trajectory2d::shortcut()
{
if (m_trajectory.empty())
{
return;
}
/* Transform each 'following' pose into the cs
* of the 'current' pose. A violation to the desired
* driving direction is found if
* x-value <= 0 and driving forward
* x-value >= 0 and driving backward
*/
Pose2d temp;
for (ContainerType::iterator current=m_trajectory.begin(),
next=current+1;
(current!=m_trajectory.end() && next!=m_trajectory.end());
++current, ++next)
{
temp = current->pose().inverse() * next->pose();
if (((m_is_forward_trajectory && (temp.translation().x() <=0.)))
|| ((!m_is_forward_trajectory) && (temp.translation().x() >=0.)))
{
erase(next);
--current; // step back to check the same place again
next = current+1;
}
}
// recalculate curvature.
if (curvatureAvailable())
{
calculateCurvature();
}
}
void Trajectory2d::fillWithTestData(double sampling_rate,
bool set_velocity,
bool movement_direction_forward)
{
m_trajectory.clear();
ExtendedPose2d pose;
isForwardTrajectory() = movement_direction_forward;
velocityAvailable() = set_velocity;
for (double x=-10.; x<50.; x+=sampling_rate)
{
double y = 0.05*sin(x-3) - 0.0001*pow(x, 3) + 1;
double yaw = atan(-0.0003*pow(x, 2) + 0.05*cos(3-x)); // yaw is atan of 1st derivative
pose.setPose(x, y, yaw);
pose.setVelocity(2. + .1*x);
if (!movement_direction_forward)
{
pose.setYaw(yaw + M_PI);
}
m_trajectory.push_back(pose);
}
calculateCurvature();
if (set_velocity)
{
m_velocity_available = true;
}
else
{
m_velocity_available = false;
}
}
void Trajectory2d::calculateCurvature()
{
if (m_trajectory.size() < 3) // trajectories with 1 or 2 points have curvature of 0.
{
for (std::size_t i=0; i<m_trajectory.size(); ++i)
{
m_trajectory[i].setCurvature(0.);
}
m_curvature_available = true;
return;
}
// iterate over all but the first and the last entries
for (std::size_t i=1; i< m_trajectory.size()-1; ++i)
{
m_trajectory[i].setCurvature(
calculateCurvature(m_trajectory[i-1].getPosition(),
m_trajectory[i].getPosition(),
m_trajectory[i+1].getPosition(),
!m_is_forward_trajectory));
}
// The first entry will have the same curvature as the second
m_trajectory[0].setCurvature(m_trajectory[1].getCurvature());
// The last entry will have the same curvature as the second last
m_trajectory[m_trajectory.size()-1].setCurvature(m_trajectory[m_trajectory.size()-2].getCurvature());
// change flag to signal the availability of curvature data
m_curvature_available = true;
}
void levelSet3Dfast(MATRIXD im, MATRIX init_mask, int max_its, double E, double T, double alpha,
MATRIX *seg0, MATRIX *tmap, MATRIXD *phi, long **ls_vols, int offset){
long dx = im.dx, dy = im.dy, dz = im.dz;
int *px, *py, *pz, *qx, *qy, *qz, k, tmap_it = 1;
MATRIX intTmp;
MATRIX intTmp2 = allocateMatrix(dx,dy,dz); // add a new temporary matrix
MATRIXD dblTmp = allocateMatrixD(dx,dy,dz);
MATRIXD gradPhi = allocateMatrixD(dx,dy,dz);
int minx, maxx, miny, maxy, minz, maxz;
*phi = allocateMatrixD(dx,dy,dz);
*seg0 = allocateMatrix(dx,dy,dz);
*tmap = allocateMatrix(dx,dy,dz);
*ls_vols = (long *)calloc(max_its,sizeof(long)); // initialized with 0
initializeMatrix(tmap,0);
initializeMatrix(seg0,0); // initialized with 0
initializeMatrix(&intTmp2,0); // initialized with 0
initializeGlobals4bwdist(dx,dy,dz,&px,&py,&pz,&qx,&qy,&qz,&intTmp);
createSignedDistanceMap(init_mask,phi,px,py,pz,qx,qy,qz,intTmp,dblTmp); // temps: intTmp, dblTmp
findLimits(init_mask,offset,&minx,&maxx,&miny,&maxy,&minz,&maxz);
for (k = 1; k <= max_its; k++){
calculateCurvature(*phi,&dblTmp,minx,maxx,miny,maxy,minz,maxz); // dblTmp keeps the curvatures
calculateF(im,dblTmp,&dblTmp,E,T,alpha,minx,maxx,miny,maxy,minz,maxz); // first dblTmp (input): curvatures, second dblTmp (output): F values
calculateGradPhi(*phi,dblTmp,&gradPhi,minx,maxx,miny,maxy,minz,maxz);
evolveCurve(phi,dblTmp,gradPhi,minx,maxx,miny,maxy,minz,maxz);
if (k % 50 == 0)
reinitializeDistanceFunction(phi,init_mask,px,py,pz,qx,qy,qz,intTmp,dblTmp); // temps: init_mask, intTmp, dblTmp
(*ls_vols)[k-1] = selectNonpositive(*phi,&intTmp2,minx,maxx,miny,maxy,minz,maxz); // use the second temporarymatrix
if (k == 1){
selectNonpositive(*phi,seg0,minx,maxx,miny,maxy,minz,maxz);
copyMatrixWithLimits(tmap,*seg0,minx,maxx,miny,maxy,minz,maxz);
findLimits(*tmap,offset,&minx,&maxx,&miny,&maxy,&minz,&maxz);
}
else if (k % 10 == 0){
selectNonpositive(*phi,&intTmp2,minx,maxx,miny,maxy,minz,maxz); // use the second temporarymatrix
updateMaps(tmap,intTmp2,*seg0,tmap_it,minx,maxx,miny,maxy,minz,maxz); // use the second temporarymatrix
copyMatrixWithLimits(seg0,intTmp2,minx,maxx,miny,maxy,minz,maxz); // use the second temporarymatrix
tmap_it++;
findLimits(*tmap,offset,&minx,&maxx,&miny,&maxy,&minz,&maxz);
}
}
selectNonpositive(*phi,seg0,0,dx-1,0,dy-1,0,dz-1);
//selectNonpositive(*phi,seg0,minx,maxx,miny,maxy,minz,maxz); ???? (consider this modification if there are more problems)
freeMatrixD(gradPhi);
freeMatrixD(dblTmp);
freeMatrix(intTmp2);
freeGlobals4bwdist(px,py,pz,qx,qy,qz,intTmp);
}
void levelSet3D(MATRIXD im, MATRIX init_mask, int max_its, double E, double T, double alpha,
MATRIX *seg0, MATRIX *tmap, MATRIXD *phi, long **ls_vols){
// some of these variables will be used as the local variables for other functions
// they are defined outside (like global variables) because we want to decrease the overhead
// corresponding to allocation and deallocation (since we have multiple calls for the same function
long dx = im.dx, dy = im.dy, dz = im.dz;
int *px, *py, *pz, *qx, *qy, *qz, k, tmap_it = 1;
MATRIX intTmp;
MATRIXD dblTmp = allocateMatrixD(dx,dy,dz);
MATRIXD gradPhi = allocateMatrixD(dx,dy,dz);
*phi = allocateMatrixD(dx,dy,dz);
*seg0 = allocateMatrix(dx,dy,dz);
*tmap = allocateMatrix(dx,dy,dz);
*ls_vols = (long *)calloc(max_its,sizeof(long)); // initialized with 0
initializeMatrix(tmap,0);
initializeGlobals4bwdist(dx,dy,dz,&px,&py,&pz,&qx,&qy,&qz,&intTmp);
createSignedDistanceMap(init_mask,phi,px,py,pz,qx,qy,qz,intTmp,dblTmp); // temps: intTmp, dblTmp
for (k = 1; k <= max_its; k++){
calculateCurvature(*phi,&dblTmp,0,dx-1,0,dy-1,0,dz-1); // dblTmp keeps the curvatures
calculateF(im,dblTmp,&dblTmp,E,T,alpha,0,dx-1,0,dy-1,0,dz-1); // first dblTmp (input): curvatures, second dblTmp (output): F values
calculateGradPhi(*phi,dblTmp,&gradPhi,0,dx-1,0,dy-1,0,dz-1);
evolveCurve(phi,dblTmp,gradPhi,0,dx-1,0,dy-1,0,dz-1);
if (k % 50 == 0)
reinitializeDistanceFunction(phi,init_mask,px,py,pz,qx,qy,qz,intTmp,dblTmp); // temps: init_mask, intTmp, dblTmp
(*ls_vols)[k-1] = selectNonpositive(*phi,&intTmp,0,dx-1,0,dy-1,0,dz-1);
if (k == 1){
selectNonpositive(*phi,seg0,0,dx-1,0,dy-1,0,dz-1);
copyMatrix(tmap,*seg0);
}
else if (k % 10 == 0){
selectNonpositive(*phi,&intTmp,0,dx-1,0,dy-1,0,dz-1);
updateMaps(tmap,intTmp,*seg0,tmap_it,0,dx-1,0,dy-1,0,dz-1);
copyMatrix(seg0,intTmp);
tmap_it++;
}
}
selectNonpositive(*phi,seg0,0,dx-1,0,dy-1,0,dz-1);
freeMatrixD(gradPhi);
freeMatrixD(dblTmp);
freeGlobals4bwdist(px,py,pz,qx,qy,qz,intTmp);
}
