<commit_before>#include <ros/ros.h>
#include <opencv/cv.h>
#include <opencv/cxcore.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <sensor_msgs/LaserScan.h>
#include <algorithm>
#include "scan2image.h"

#include "autoware_msgs/ScanImage.h"
#include "autoware_msgs/projection_matrix.h"
#include <sensor_msgs/CameraInfo.h>

#if 1 // AXE
#define XSTR(x) #x
#define STR(x) XSTR(x)
#endif

static cv::Mat cameraExtrinsicMat;
static cv::Mat cameraMat;
static cv::Mat distCoeff;
static cv::Size imageSize;

ros::Publisher transformed_point_data;
static bool isProjection;
static bool isIntrinsic;
Scan_image scan_image;

void trans_depth_points_to_image_points(Scan_points_dataset* scan_points_dataset, Image_points_dataset* image_points_dataset)
{
    float camera_x;
    float camera_y;
    float camera_z;
    int i;

    for(i = 0; i < (int)scan_points_dataset->scan_points.x.size(); i++) {

        /*
         * Coordinate transformation. Change from laser range finder coordinate to camera coordinate
         */
        camera_x = (cameraExtrinsicMat.at<double>(0,0) * scan_points_dataset->scan_points.x.at(i)*1000
                    + cameraExtrinsicMat.at<double>(0,1) * scan_points_dataset->scan_points.y.at(i)*1000
                    + cameraExtrinsicMat.at<double>(0,2) * scan_points_dataset->scan_points.z.at(i)*1000)
            + (cameraExtrinsicMat.at<double>(0,3));
        camera_y = (cameraExtrinsicMat.at<double>(1,0) * scan_points_dataset->scan_points.x.at(i)*1000
                    + cameraExtrinsicMat.at<double>(1,1) * scan_points_dataset->scan_points.y.at(i)*1000
                    + cameraExtrinsicMat.at<double>(1,2) * scan_points_dataset->scan_points.z.at(i)*1000)
            + (cameraExtrinsicMat.at<double>(1,3));
        camera_z = (cameraExtrinsicMat.at<double>(2,0) * scan_points_dataset->scan_points.x.at(i)*1000
                    + cameraExtrinsicMat.at<double>(2,1) * scan_points_dataset->scan_points.y.at(i)*1000
                    + cameraExtrinsicMat.at<double>(2,2) * scan_points_dataset->scan_points.z.at(i)*1000)
            + (cameraExtrinsicMat.at<double>(2,3));
        if (camera_z >= 0.0) {
            /*
             * Projection transformation. Change from camera coordinate to image coordinate
             */
            image_points_dataset->image_points.x.push_back((camera_x * cameraMat.at<double>(0,0) / camera_z) + cameraMat.at<double>(0,2));
            image_points_dataset->image_points.y.push_back((camera_y * cameraMat.at<double>(1,1) / camera_z) + cameraMat.at<double>(1,2));
            /*
             * Calculate euclidean distance from the camera to objects<commit_msg>Fixed zero division in scan2image<commit_after>#include <ros/ros.h>
#include <opencv/cv.h>
#include <opencv/cxcore.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <sensor_msgs/LaserScan.h>
#include <algorithm>
#include "scan2image.h"

#include "autoware_msgs/ScanImage.h"
#include "autoware_msgs/projection_matrix.h"
#include <sensor_msgs/CameraInfo.h>

#if 1 // AXE
#define XSTR(x) #x
#define STR(x) XSTR(x)
#endif

static cv::Mat cameraExtrinsicMat;
static cv::Mat cameraMat;
static cv::Mat distCoeff;
static cv::Size imageSize;

ros::Publisher transformed_point_data;
static bool isProjection;
static bool isIntrinsic;
Scan_image scan_image;

void trans_depth_points_to_image_points(Scan_points_dataset* scan_points_dataset, Image_points_dataset* image_points_dataset)
{
    float camera_x;
    float camera_y;
    float camera_z;
    int i;

    for(i = 0; i < (int)scan_points_dataset->scan_points.x.size(); i++) {

        /*
         * Coordinate transformation. Change from laser range finder coordinate to camera coordinate
         */
        camera_x = (cameraExtrinsicMat.at<double>(0,0) * scan_points_dataset->scan_points.x.at(i)*1000
                    + cameraExtrinsicMat.at<double>(0,1) * scan_points_dataset->scan_points.y.at(i)*1000
                    + cameraExtrinsicMat.at<double>(0,2) * scan_points_dataset->scan_points.z.at(i)*1000)
            + (cameraExtrinsicMat.at<double>(0,3));
        camera_y = (cameraExtrinsicMat.at<double>(1,0) * scan_points_dataset->scan_points.x.at(i)*1000
                    + cameraExtrinsicMat.at<double>(1,1) * scan_points_dataset->scan_points.y.at(i)*1000
                    + cameraExtrinsicMat.at<double>(1,2) * scan_points_dataset->scan_points.z.at(i)*1000)
            + (cameraExtrinsicMat.at<double>(1,3));
        camera_z = (cameraExtrinsicMat.at<double>(2,0) * scan_points_dataset->scan_points.x.at(i)*1000
                    + cameraExtrinsicMat.at<double>(2,1) * scan_points_dataset->scan_points.y.at(i)*1000
                    + cameraExtrinsicMat.at<double>(2,2) * scan_points_dataset->scan_points.z.at(i)*1000)
            + (cameraExtrinsicMat.at<double>(2,3));
        if (camera_z > 0.0) {
            /*
             * Projection transformation. Change from camera coordinate to image coordinate
             */
            image_points_dataset->image_points.x.push_back((camera_x * cameraMat.at<double>(0,0) / camera_z) + cameraMat.at<double>(0,2));
            image_points_dataset->image_points.y.push_back((camera_y * cameraMat.at<double>(1,1) / camera_z) + cameraMat.at<double>(1,2));
            /*
             * Calculate euclidean distance from the camera to objects
{'fraction_non_alphanumeric': 0.13539617241996066, 'fraction_numerical': 0.027007690931854768, 'mean_word_length': 3.587366694011485, 'pattern_counts': {'":': 0, '<': 65, '<?xml version=': 0, '>': 89, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3874796', 'n_tokens_mistral': 2017, 'n_tokens_neox': 1918, 'n_words': 297}