Detección de líneas negras en una imagen en C++ con librería CImg.h

[fernandoasr.8]

Buen día, sigo un poco con el tema de la librería CImg.h, el problema que ahora se me presenta es que teniendo dos puntos INICIO y FINAL en la imagen (cabe señalar que se trata una imagen en blanco con lineas negras), se unan con una linea recta, y si encuentra un obstáculo en su camino, lo rodeé y volver a seguir con su camino a su punto final.
Aquí una representación de lo que quiero conseguir.



He tratado y no he podido hacer que el punto rodeé el obstáculo para seguir con su destino.
Adjunto el código que llevo hasta el momento.

Código: [Seleccionar]

#include "CImg.h"
#include <iostream>
using namespace std;
using namespace cimg_library;

 CImg<unsigned char> imagen;
unsigned char rojo[3] = {255, 0, 0};
unsigned char amarillo[3] = {233, 251,11};
unsigned char azul[3] = {0, 5, 198};
unsigned char black[3] = {0, 0, 0};
unsigned char verde[3] = {0, 255, 0};
const float opacidad = 1;
int rango;
float equis1,ye1,equis2,ye2;
int anteriorX,anteriorY;

int opcion;

int main()
{
    int c;
    char o;
cout<<"Obstaculos \n \n";
do
    {
        system("cls");
        cout<<"Ingresar el nombre del mapa: \n";
        string image1;
        cin>>image1;
        cimg::exception_mode(0);
        try
        {
            c=0;
            imagen.load(image1.c_str());
        }
        catch(CImgException &e)
        {
            c=1;
            cout<<"\n Imagen no encontrada"<<endl;
            cout<<"\n Desea ingresar otra imagen? S/N : ";
            char res;
            cin>>res;
            if(res=='N')
            {
                exit(0);
            }
        }
    }while(c==1);

    int width=imagen.width();
    int height=imagen.height();
    int x,y;
    int a1,a2,a3;

            CImgDisplay disp;
            cout<<"Coordenadas Iniciales: "<<endl;
            cout<<"Ingresa la primer coordenada x en los rangos de: 0 a "<< width << endl;
            cin>>equis1;
            cout<<"Ingresa la primer coordenada y en los rangos de: 0 a " << height<<endl;
            cin>>ye1;
            cout<<"Coordenadas Finales: "<<endl;
            cout<<"Ingresa la segunda coordenada x en los rangos de: 0 a "<< width << endl;
            cin>>equis2;
            cout<<"Ingresa la segunda coordenada y en los rangos de: 0 a " << height<<endl;
            cin>>ye2;
            cout<<"RANGO DEL SENSOR "<<endl;
            cout<<"Ingresa el rango de sensor de 5 a 20: " << endl;
            cin>>rango;

            imagen.draw_circle(equis1,ye1,4,azul,1);
            imagen.draw_circle(equis2,ye2,4,amarillo,1);
            if(equis1<equis2)
            {
                for(x = equis1; x<=equis2; x+=rango)
                {
                    y=((ye2-ye1))/(equis2-equis1) * (x-equis1) + ye1;
                    disp.display(imagen);
                    imagen.draw_circle(x,y,2,rojo,1,2);
                    a1=imagen(x,y,0,0);
                    a2=imagen(x,y,0,1);
                    a3=imagen(x,y,0,2);
                    int circulo[3]={a1,a2,a3};
                    if(circulo[0]==0 && circulo[1]==0 || circulo[2]==0)
                    {
                        cout<<"OBSTACULO DETECTADO 1 "<<endl;
                        break;
                    }
                }
            }
            else if(equis1>equis2)
            {
                for(x=equis1;x>=equis2;x-=rango)
                {
                     y=((ye2-ye1))/(equis2-equis1) * (x-equis1) + ye1;
                    imagen.draw_circle(x,y,2,rojo,1,2);
                    disp.display(imagen);
                    a1=imagen(x,y,0,0);
                    a2=imagen(x,y,0,1);
                    a3=imagen(x,y,0,2);
                    int circulo[3]={a1,a2,a3};
                    if(circulo[0]==0 && circulo[1]==0 || circulo[2]==0)
                    {
                        cout<<"OBSTACULO DETECTADO 2 "<<endl;
                        break;
                    }
                }
            }
            else{
                if(ye1<ye2)
                {
                    y=((ye2-ye1))/(equis2-equis1) * (x-equis1) + ye1;
                    x=equis1;
                    for(y=ye1; y<=ye2; y+=rango)
                    {
                     imagen.draw_circle(x,y,2,rojo,1,2);
                    disp.display(imagen);
                    a1=imagen(x,y,0,0);
                    a2=imagen(x,y,0,1);
                    a3=imagen(x,y,0,2);
                    int circulo[3]={a1,a2,a3};
                    if(circulo[0]==0 && circulo[1]==0 || circulo[2]==0)
                    {
                        cout<<"OBSTACULO DETECTADO 3 "<<endl;
                        break;
                    }
                    }
                }
                else
                {
                     y=((ye2-ye1))/(equis2-equis1) * (x-equis1) + ye1;
                    x=equis1;
                    for(y=ye1; y>=ye2; y-=rango)
                    imagen.draw_circle(x,y,2,rojo,1,2);
                    disp.display(imagen);
                    a1=imagen(x,y,0,0);
                    a2=imagen(x,y,0,1);
                    a3=imagen(x,y,0,2);
                    int circulo[3]={a1,a2,a3};
                    if(circulo[0]==0 && circulo[1]==0 || circulo[2]==0)
                    {
                        cout<<"OBSTACULO DETECTADO 4 "<<endl;
                    }
                }
            }
            while (!disp.is_closed())
            {
                disp.wait();
            }
    return 0;
}



Otro aspecto que no pude conseguir, fue dibujar la recta con ayuda de ángulos. Espero pueda recibir aunque sea una pequeña ayuda con este programa.
De antemano muchas gracias.

Adjunto también la imagen de prueba de una imagen blanca con un obstáculo.

[Mastermind]

Hola, el problema parece complejo por lo que parece adecuado tratar de resolverlo por partes. Podrías empezar así:

1) Crear el programa que una con una recta los dos puntos, ignorando los obstáculos

2) Crear un programa que una con una recta los dos puntos y que muestre por pantalla las coordenadas en que existe una intersección con un obstáculo.

3) El siguiente paso sería, encontrado un obstáculo, determinar el punto de comienzo del rodeo del obstáculo y el punto de terminación de rodeo del obstáculo. Ten en cuenta que una vez acabas de rodear un obstáculo, el problema queda reducido a repetir el problema pero tomando como punto inicial el punto de salida del rodeo y el punto final (esto invita a usar recursión, aunque supongo que se puede hacer de otras maneras).

La lógica para rodear un obstáculo podría ser esta: dibujar la traza a una distancia d del obstáculo hasta que dicha traza intersecte con la línea recta teórica que existe entre el punto inicial y el punto final. Cuando se llega a esa intersección, continuar siguiendo la recta.

Parece que sería útil trabajar con la ecuación de la recta y = mx + b, pero hay que verlo paso a paso

Saludos!!