int main(int argc, char **argv) {
static char * tmp = NULL;
static int tmpi;
ros::init(tmpi, &tmp, "image_processing", ros::init_options::NoSigintHandler);
ros::NodeHandle nh;
ImageProcessing imageProcessing(nh);
int i = 0;
ros::Rate loop_rate(10);
loop_rate.sleep();
while (imageProcessing.sourceImage.size().height == 0){
ros::spinOnce();
loop_rate.sleep();
}
while (ros::ok()){
cv::Mat image = imageProcessing.sourceImage;
cv::Mat out;
cv::cvtColor(image, out, CV_RGB2GRAY);
bool a = cv::findChessboardCorners(out, cvSize(5,4), imageProcessing.imagePoints);
if (a == true){
imageProcessing.found = 1;
}
else{
imageProcessing.found = 0;
}
if(imageProcessing.found == 1){
++i;
cv::solvePnP(imageProcessing.objectPoints, imageProcessing.imagePoints,
imageProcessing.cameraMatrix, imageProcessing.distCoeffs,
imageProcessing.rvec, imageProcessing.tvec);//solvePnP
cv::Mat_<double> rotationMatrix;
cv::Rodrigues(imageProcessing.rvec, rotationMatrix);//change rvec to matrix
cv::Mat pattern_pose =
(cv::Mat_<double>(4, 4) << rotationMatrix(0, 0), rotationMatrix(0, 1), rotationMatrix(
0, 2), imageProcessing.tvec(0), rotationMatrix(1, 0), rotationMatrix(
1, 1), rotationMatrix(1, 2), imageProcessing.tvec(1), rotationMatrix(
2, 0), rotationMatrix(2, 1), rotationMatrix(2, 2), imageProcessing
.tvec(2), 0, 0, 0, 1);//final matrix (grid position in camera frame)
imageProcessing.msg.error = imageProcessing.tvec(0);
}
if(imageProcessing.found == 0){
imageProcessing.msg.error = 0;
}
imageProcessing.msg.found = imageProcessing.found;
imageProcessing.grid_info_pub.publish(imageProcessing.msg);//publish message
ros::spinOnce();
}
return 0;
}
int main(int argc, char **argv) {
std::ofstream pnp_time;
pnp_time.open ("pnp_time.txt");
std::ofstream recognition_time;
recognition_time.open ("recognition_time.txt");
static char * tmp = NULL;
static int tmpi;
ros::init(tmpi, &tmp, "image_processing", ros::init_options::NoSigintHandler);
ros::NodeHandle nh;
ImageProcessing imageProcessing(nh);
int i = 0;
while (/*ros::ok()*/ i < 101 ){
auto start_recognition= std::chrono::high_resolution_clock::now(); //start time count
imageProcessing.imagePoints = imageProcessing.Generate2DPoints(); //updating image points
auto finish_recognition = std::chrono::high_resolution_clock::now(); //stop time count
if(imageProcessing.found == 1){
recognition_time << std::chrono::duration_cast<std::chrono::nanoseconds>(finish_recognition-start_recognition).count()<<"\n"; //save time
}
auto start_pnp = std::chrono::high_resolution_clock::now(); //start time count
if(imageProcessing.found == 1){
++i;
cv::solvePnP(imageProcessing.objectPoints, imageProcessing.imagePoints,
imageProcessing.cameraMatrix, imageProcessing.distCoeffs,
imageProcessing.rvec, imageProcessing.tvec);//solvePnP
cv::Mat_<double> rotationMatrix;
cv::Rodrigues(imageProcessing.rvec, rotationMatrix);//change rvec to matrix
cv::Mat pattern_pose =
(cv::Mat_<double>(4, 4) << rotationMatrix(0, 0), rotationMatrix(0, 1), rotationMatrix(
0, 2), imageProcessing.tvec(0), rotationMatrix(1, 0), rotationMatrix(
1, 1), rotationMatrix(1, 2), imageProcessing.tvec(1), rotationMatrix(
2, 0), rotationMatrix(2, 1), rotationMatrix(2, 2), imageProcessing
.tvec(2), 0, 0, 0, 1);//final matrix (grid position in camera frame)
imageProcessing.msg.error = imageProcessing.tvec(0);
}
if(imageProcessing.found == 0){
imageProcessing.msg.error = 0;
}
imageProcessing.msg.found = imageProcessing.found;
imageProcessing.grid_info_pub.publish(imageProcessing.msg);//publish message
auto finish_pnp = std::chrono::high_resolution_clock::now(); //stop time count
if(imageProcessing.found == 1){
pnp_time << std::chrono::duration_cast<std::chrono::nanoseconds>(finish_pnp-start_pnp).count()<<"\n"; //save time
}
ros::spinOnce();
}
pnp_time.close();
recognition_time.close();
return 0;
}
void imageConv(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
img = new IplImage(cv_ptr->image);
imageProcessing(img);
}
void display()
{
imageProcessing();
glClearColor(0, 0, 0, 0);
glClear(GL_COLOR_BUFFER_BIT);
glPixelZoom(1, -1);
glRasterPos2i(0 , 0);
rgbImage = cvCloneImage(image);
//IPL形式の画像（BGR）をOpenGL形式（RGB）に変換
if (image->nChannels >= 2)
{
// BGR(A) -> RGB(A)
for (int i = 0; i < image->width * image->height * image->nChannels; i = i + image->nChannels) {
rgbImage->imageData[i] = image->imageData[i + 2];
rgbImage->imageData[i + 2] = image->imageData[i];
}
}
//チャンネルに応じて書き込み方を変更
switch (image->nChannels)
{
case 1:
glDrawPixels(image->width, image->height, GL_LUMINANCE, GL_UNSIGNED_BYTE, image->imageData);
break;
case 3:
glDrawPixels(image->width, image->height, GL_RGB, GL_UNSIGNED_BYTE, rgbImage->imageData);
break;
case 4:
glDrawPixels(image->width, image->height, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage->imageData);
break;
default:
break;
}
//出力
glFlush();
}
int main(int argc, char **argv) {
static char * tmp = NULL;
static int tmpi;
ros::init(tmpi, &tmp, "image_processing", ros::init_options::NoSigintHandler);
ros::NodeHandle nh;
ImageProcessing imageProcessing(nh);
while (ros::ok()) {
//updating image points
imageProcessing.imagePoints = imageProcessing.Generate2DPoints();
//solvePnP
cv::solvePnP(imageProcessing.objectPoints, imageProcessing.imagePoints,
imageProcessing.cameraMatrix, imageProcessing.distCoeffs,
imageProcessing.rvec, imageProcessing.tvec);
//change rvec to matrix
cv::Mat_<double> rotationMatrix;
cv::Rodrigues(imageProcessing.rvec, rotationMatrix);
//final matrix (grid position in camera frame)
cv::Mat pattern_pose =
(cv::Mat_<double>(4, 4) << rotationMatrix(0, 0), rotationMatrix(0, 1), rotationMatrix(
0, 2), imageProcessing.tvec(0), rotationMatrix(1, 0), rotationMatrix(
1, 1), rotationMatrix(1, 2), imageProcessing.tvec(1), rotationMatrix(
2, 0), rotationMatrix(2, 1), rotationMatrix(2, 2), imageProcessing
.tvec(2), 0, 0, 0, 1);
/*printf("[ %f %f %f %f\n %f %f %f %f\n %f %f %f %f\n %f %f %f %f]\n ", rotationMatrix(0,0), rotationMatrix(0,1), rotationMatrix(0,2), imageProcessing.tvec(0),
rotationMatrix(1,0), rotationMatrix(1,1), rotationMatrix(1,2), imageProcessing.tvec(1),
rotationMatrix(2,0), rotationMatrix(2,1), rotationMatrix(2,2), imageProcessing.tvec(2),
0.0, 0.0, 0.0, 1.0);*/
//push pattern_pose to vector
std::vector<double> pattern_pose_vector;
pattern_pose_vector.push_back(rotationMatrix(0, 0));
pattern_pose_vector.push_back(rotationMatrix(0, 1));
pattern_pose_vector.push_back(rotationMatrix(0, 2));
pattern_pose_vector.push_back(imageProcessing.tvec(0));
pattern_pose_vector.push_back(rotationMatrix(1, 0));
pattern_pose_vector.push_back(rotationMatrix(1, 1));
pattern_pose_vector.push_back(rotationMatrix(1, 2));
pattern_pose_vector.push_back(imageProcessing.tvec(1));
pattern_pose_vector.push_back(rotationMatrix(2, 0));
pattern_pose_vector.push_back(rotationMatrix(2, 1));
pattern_pose_vector.push_back(rotationMatrix(2, 2));
pattern_pose_vector.push_back(imageProcessing.tvec(2));
pattern_pose_vector.push_back(0.0);
pattern_pose_vector.push_back(0.0);
pattern_pose_vector.push_back(0.0);
pattern_pose_vector.push_back(1.0);
//std::fstream plik;
//plik.open( "/home/mwegiere/DC.txt", std::ios::app | std::ios::out);
//for (int i=0; i<16; ++i)
//std::cout<<pattern_pose_vector[3]<<std::endl;
//plik.close();
//create message
imageProcessing.msg.matrix = pattern_pose_vector;
imageProcessing.msg.found = imageProcessing.found;
imageProcessing.msg.out_time_nsec_pocz = 0;
imageProcessing.msg.out_time_sec_pocz = 0;
imageProcessing.msg.out_time_nsec_kon = 0;
imageProcessing.msg.out_time_sec_kon = 0;
//publish message
imageProcessing.grid_info_pub.publish(imageProcessing.msg);
ros::spinOnce();
}
return 0;
}
/*
* Proceso 3
*/
int main(int argc, char** argv) {
struct sockaddr_in direccion;
FILE *filenew;
char bufferr[1024];
char peticion[220];
char nameImage[20];
int contadorImagen;
unsigned long fileLen;
int params[2];
int rec, rest, kl;
int sock, i, envio, bytes;
char toWriteFile[BUF_SIZE];
char nameFile[20];
char dataFile[20];
int aux = 0; //variable donde se almacenará el valor del archivo contador.log
strcpy(nameFile, "contador.log");
getFileData(nameFile, dataFile);
if (strlen(dataFile) > 0) {
if (strcmp(dataFile, "error") < 0) {
aux = atoi(dataFile);
}
}
contadorImagen = aux;
while (contadorImagen < 5) {
sock = socket(AF_INET, SOCK_STREAM, 0);
if (sock < 0)
printf("ERROR: Socket no construido.");
else
printf("Socket creado.");
sprintf(nameImage, "im%d.bmp", contadorImagen);
bzero((char*) &direccion, sizeof (direccion));
direccion.sin_family = AF_INET;
direccion.sin_port = htons(3393);
direccion.sin_addr.s_addr = inet_addr("192.168.1.69");
printf("--------CLIENTE QUE PIDE IMAGEN----------------\n");
if (connect(sock, (struct sockaddr*) &direccion, sizeof (direccion)) < 0) {//conectando con servidor
printf("\n ERROR CONNECT: no connect para pedir imagen.");
sleep(1);
} else {
strcpy(peticion, "get_imagen");
if (send(sock, peticion, strlen(peticion), 0) == -1)
printf("ERROR SEND: Error al enviar solicitud.\n");
//COMENZANDO A RECIBIR IMAGEN
char mensaje_r[10];
if (recv(sock, mensaje_r, sizeof (mensaje_r), 0) == -1) {
printf("Error recibiendo respuesta");
}
//Mientras no recivamos si preguntamos.
if (strcmp(mensaje_r, "SI") != 0) {
sleep(1);
continue;
}
char mensaje_e[] = "LISTO";
send(sock, mensaje_e, sizeof (mensaje_e), 0);
//recibiendo imagen
receive_image(sock,nameImage);
/*
bytes = recv(sock, (char *) ¶ms, 2 * sizeof (int), 0);
if (bytes < 0)
printf("ERROR RECV: no se recibieron caracteristicas de la imagen.\n");
else
printf("Caracteristicas recibidas en RECV\n");
rec = params[0]; //recivido
rest = params[1]; //restante
printf("Recorrer archivo %d veces \n", rec);
printf("Resta de b %d \n", rest);
//writeDataFile(nameImage,sock,params);
filenew = fopen(nameImage, "w");
for (kl = 0; kl < rec; kl++) {
recv(sock, (char *) &bufferr, sizeof (bufferr), 0);
fwrite(bufferr, 1024, sizeof (char), filenew);
memset(bufferr, 0, strlen(bufferr));
printf("Escrito \n");
}
if (rest > 0) {
recv(sock, (char *) &bufferr, rest * sizeof (char), 0);
fwrite(bufferr, rest, sizeof (char), filenew);
memset(bufferr, 0, strlen(bufferr));
}
fclose(filenew);
*/
printf("\nEscrito totalmente \n");
contadorImagen++;
close(sock);
sprintf(toWriteFile, "%d", contadorImagen);
setFileData(nameFile, toWriteFile);
imageProcessing(nameImage, contadorImagen);
//Enviar petición para borrar la imagen al servidor
sock = socket(AF_INET, SOCK_STREAM, 0);
if (sock < 0)
printf("ERROR: Socket no construido.");
else
printf("Socket creado.");
bzero((char*) &direccion, sizeof (direccion));
direccion.sin_family = AF_INET;
direccion.sin_port = htons(3393);
direccion.sin_addr.s_addr = inet_addr("192.168.1.69");
printf("--------CLIENTE QUE PIDE BORRAR IMAGEN----------------\n");
if (connect(sock, (struct sockaddr*) &direccion, sizeof (direccion)) < 0) {//conectando con servidor
printf("\n ERROR CONNECT: no connect para pedir imagen.");
sleep(1);
} else {
memset(peticion, 0, sizeof (peticion));
strcpy(peticion, "borrar_imagen");
if (send(sock, peticion, strlen(peticion), 0) == -1)
printf("ERROR SEND: Error al enviar solicitud.\n");
if (recv(sock, (char *) &bufferr, sizeof (bufferr), 0) < 0) {
printf("ERROR RECV: no se recibieron caracteristicas de la imagen.\n");
}
if (strcmp(bufferr, "OK") == 0) {
printf("Imagen borrada\n");
} else if (strcmp(bufferr, "NO") == 0) {
printf("Permiso denegado\n");
}
close(sock);
}
}
}
return (EXIT_SUCCESS);
}
int main(int argc, char **argv) {
//std::ofstream pnp_time;
//pnp_time.open ("pnp_time.txt");
//std::ofstream recognition_time;
//recognition_time.open ("recognition_time.txt");
static char * tmp = NULL;
static int tmpi;
ros::init(tmpi, &tmp, "image_processing", ros::init_options::NoSigintHandler);
ros::NodeHandle nh;
ImageProcessing imageProcessing(nh);
ros::Rate loop_rate(10);
loop_rate.sleep();
while (imageProcessing.sourceImage.size().height == 0){
ros::spinOnce();
loop_rate.sleep();
}
std::cout<<imageProcessing.sourceImage.size().height<<std::endl;
// /ros::spinOnce();
loop_rate.sleep();
while (i < 101) {
//updating image points
//auto start_recognition= std::chrono::high_resolution_clock::now();
//imageProcessing.imagePoints = imageProcessing.Generate2DPoints();
cv::Mat image = imageProcessing.sourceImage;
cv::Mat out;
cv::cvtColor(image, out, CV_RGB2GRAY);
std::cout<<out.size().height<<std::endl;
bool a = cv::findChessboardCorners(out, cvSize(5,4), imageProcessing.imagePoints);
std::cout<<a<<std::endl;
auto finish_recognition = std::chrono::high_resolution_clock::now();
recognition_time << std::chrono::duration_cast<std::chrono::nanoseconds>(finish_recognition-start_recognition).count()<<"\n";
//solvePnP
auto start_pnp = std::chrono::high_resolution_clock::now();
cv::solvePnP(imageProcessing.objectPoints, imageProcessing.imagePoints,
imageProcessing.cameraMatrix, imageProcessing.distCoeffs,
imageProcessing.rvec, imageProcessing.tvec);
//change rvec to matrix
cv::Mat_<double> rotationMatrix;
cv::Rodrigues(imageProcessing.rvec, rotationMatrix);
//final matrix (grid position in camera frame)
cv::Mat pattern_pose =
(cv::Mat_<double>(4, 4) << rotationMatrix(0, 0), rotationMatrix(0, 1), rotationMatrix(
0, 2), imageProcessing.tvec(0), rotationMatrix(1, 0), rotationMatrix(
1, 1), rotationMatrix(1, 2), imageProcessing.tvec(1), rotationMatrix(
2, 0), rotationMatrix(2, 1), rotationMatrix(2, 2), imageProcessing
.tvec(2), 0, 0, 0, 1);
imageProcessing.msg.error = imageProcessing.tvec(0);
imageProcessing.msg.found = imageProcessing.found;
//publish message
imageProcessing.grid_info_pub.publish(imageProcessing.msg);
auto finish_pnp = std::chrono::high_resolution_clock::now();
pnp_time << std::chrono::duration_cast<std::chrono::nanoseconds>(finish_pnp-start_pnp).count()<<"\n";
ros::spinOnce();
}
//pnp_time.close();
//recognition_time.close();
return 0;
}
