double bob::ip::gabor::JetStatistics::logLikelihood(const boost::shared_ptr<bob::ip::gabor::Jet> jet, bool estimate_phase, const blitz::TinyVector<double,2>& offset) const{
double q_phase = 0.;
double factor = 1.;
if (estimate_phase){
// compute the disparity for the given jet
auto disp = disparity(jet);
// correct disparity (which was computed from integer location)
disp[0] -= offset[0] - (int)offset[0];
disp[1] -= offset[1] - (int)offset[1];
// .. and the phase part
auto kernels = m_gwt->waveletFrequencies();
auto abs = jet->abs(), phase = jet->phase();
for (int j = jet->length(); j--;){
q_phase += sqr(adjust_phase(phase(j) + kernels[j][0] * disp[0] + kernels[j][1] * disp[1] - m_meanPhase(j))) / m_varPhase(j) * abs(j) / m_varAbs(j);
}
// q_phase *= blitz::sum(m_varPhase);
factor = 2.;
}
// compute quality measure
// .. absolute part
blitz::Array<double,1> diff(jet->abs() - m_meanAbs);
double q_abs = blitz::sum(diff*diff / m_varAbs);
// double q_abs = blitz::sum(diff*diff / m_varAbs) * blitz::sum(m_varAbs);
return -(q_abs + q_phase)/(factor*jet->length());
}
blitz::TinyVector<double,2> bob::ip::gabor::JetStatistics::disparity(const boost::shared_ptr<bob::ip::gabor::Jet> jet) const{
if (!m_gwt) throw std::runtime_error("The Gabor wavelet transform class has not been set jet");
if (m_gwt->numberOfWavelets() != jet->length())
throw std::runtime_error((boost::format("The given Gabor jet is of length %d, but the transform has %d wavelets; forgot to set your custom Transform") % jet->length() % m_gwt->numberOfWavelets()).str());
// compute confidences and phase differences once
m_confidences.resize(m_meanAbs.shape());
m_phaseDifferences.resize(m_meanPhase.shape());
m_confidences = m_meanAbs * jet->abs();
m_phaseDifferences = m_meanPhase - jet->phase();
double gamma_y_y = 0., gamma_y_x = 0., gamma_x_x = 0., phi_y = 0., phi_x = 0.;
blitz::TinyVector<double,2> disparity(0., 0.);
auto kernels = m_gwt->waveletFrequencies();
// iterate through the Gabor jet **backwards** (from highest scale to lowest scale)
for (int j = jet->length()-1, scale = m_gwt->numberOfScales(); scale--;){
for (int direction = m_gwt->numberOfDirections(); direction--; --j){
const double kjy = kernels[j][0], kjx = kernels[j][1];
const double conf = m_confidences(j), diff = m_phaseDifferences(j), var = m_varPhase(j);
// totalize Gamma matrix
gamma_y_y += conf * kjy * kjy / var;
gamma_y_x += conf * kjy * kjx / var;
gamma_x_x += conf * kjx * kjx / var;
// totalize phi vector
// estimate the number of cycles that we are off (using the current estimation of the disparity
double n = round((diff - disparity[0] * kjy - disparity[1] * kjx) / (2.*M_PI));
// totalize corrected phi vector elements
phi_y += conf * (diff - n * 2. * M_PI) * kjy / var;
phi_x += conf * (diff - n * 2. * M_PI) * kjx / var;
}
// re-calculate disparity as d=\Gamma^{-1}\Phi of the (low frequency) wavelet scales that we used up to now
double gamma_det = gamma_x_x * gamma_y_y - sqr(gamma_y_x);
disparity[0] = (gamma_x_x * phi_y - gamma_y_x * phi_x) / gamma_det;
disparity[1] = (gamma_y_y * phi_x - gamma_y_x * phi_y) / gamma_det;
}
return disparity;
}
int main(int argc, char* argv[]) {
//Load Matrix Q
cv::FileStorage fs("/home/bodereau/Bureau/OpenCVReprojectImageToPointCloud/Q.xml", cv::FileStorage::READ);
cv::Mat Q;
pcl::visualization::CloudViewer viewer ("3D Viewer");
fs["Q"] >> Q;
//If size of Q is not 4x4 exit
if (Q.cols != 4 || Q.rows != 4)
{
std::cerr << "ERROR: Could not read matrix Q (doesn't exist or size is not 4x4)" << std::endl;
return 1;
}
//Get the interesting parameters from Q
double Q03, Q13, Q23, Q32, Q33;
Q03 = Q.at<double>(0,3);
Q13 = Q.at<double>(1,3);
Q23 = Q.at<double>(2,3);
Q32 = Q.at<double>(3,2);
Q33 = Q.at<double>(3,3);
std::cout << "Q(0,3) = "<< Q03 <<"; Q(1,3) = "<< Q13 <<"; Q(2,3) = "<< Q23 <<"; Q(3,2) = "<< Q32 <<"; Q(3,3) = "<< Q33 <<";" << std::endl;
cv::Size size(752, 480);
vision::StereoRectifier rectifier(size);
rectifier.load_intrinsic_params("/home/bodereau/Bureau/intrinsics.yml");
rectifier.load_extrinsic_params("/home/bodereau/Bureau/extrinsics.yml");
rectifier.stereo_rectify();
rectifier.dump_rectification_info();
int idx = 220;
//time
clock_t start,other_clock,clock2,clock4;
float time,time2,timtotal,time3,time4;
timtotal = 0.0;
start = clock();
//~time
cv::namedWindow("vigne");
cv::namedWindow("bord");
while(idx++ < 500) {
//printf("boucle : %d \n",idx);
std::string ref = std::to_string(idx);
std::string leftImageFilename = "/home/bodereau/Bureau/tiff2/" + ref + "_l.tiff";
std::string rightImageFilename = "/home/bodereau/Bureau/tiff2/" + ref + "_r.tiff";
cv::Mat g1, g2;
clock2 = clock();
cv::Size size_rectified = rectifier.get_recified_size();
cv::Mat3b mat_r = cv::imread(rightImageFilename);
cv::Mat3b mat_l = cv::imread(leftImageFilename);
cv::Mat3b rectified_l = cv::Mat3b::zeros(size_rectified);
cv::Mat3b rectified_r = cv::Mat3b::zeros(size_rectified);
rectifier.generate_rectified_mat(mat_l, mat_r, rectified_l, rectified_r);
DTSStereo dtsStereo;
/*int test = dtsStereo.computeLab(rectified_l.data, size_rectified.area(), size_rectified.width);
int test2 = dtsStereo.compute(rectified_l.data, size_rectified.area(), size_rectified.width);*/
int height_without_sky = dtsStereo.computeHisto(rectified_l, size_rectified.area(), size_rectified.width);
cv::namedWindow("img test1", CV_WINDOW_AUTOSIZE);
printf("height without sky %d \n", height_without_sky);
clock2 = clock() - clock2;
time3 = ((float) (clock2)) / CLOCKS_PER_SEC;
printf("time for rectifie : %f \n", time3);
other_clock = clock();
cv::cvtColor(rectified_l, g1, CV_BGR2GRAY);
cv::cvtColor(rectified_r, g2, CV_BGR2GRAY);
cv::Mat left_image_rectified_crop, right_image_rectified_crop;
/*cv::Rect win(1, 339-test, 560, test);
cv::Rect win2(1,339-test2,560,test2);*/
cv::Rect win3(1, 339 - height_without_sky, 560, height_without_sky);
rectified_l(win3).copyTo(left_image_rectified_crop);
rectified_r(win3).copyTo(right_image_rectified_crop);
//printf("convert to png::image left done \n");
//png::image<png::rgb_pixel> rightImage("right.png");
other_clock = clock() - other_clock;
time2 = ((float) (other_clock)) / CLOCKS_PER_SEC;
timtotal = timtotal + time2;
printf("time lost in png make : %f \n", time2);
//~time*/
/*idxVect = 0;
rightImage.write("rgb2.png");*/
//printf("convert to png::image right done \n");
SPSStereo sps;
sps.setIterationTotal(outerIterationTotal, innerIterationTotal);
sps.setWeightParameter(lambda_pos, lambda_depth, lambda_bou, lambda_smo);
sps.setInlierThreshold(lambda_d);
sps.setPenaltyParameter(lambda_hinge, lambda_occ, lambda_pen);
cv::Mat segmentImage(600, 600, CV_16UC1);
std::vector <std::vector<double>> disparityPlaneParameters;
std::vector <std::vector<int>> boundaryLabels;
//printf("go to compute \n");
other_clock = clock();
cv::Mat disparity(600, 600, CV_16UC1);
sps.compute(superpixelTotal, left_image_rectified_crop, right_image_rectified_crop, segmentImage, disparity, disparityPlaneParameters, boundaryLabels);
/*cv::StereoSGBM sgbm;
sgbm.SADWindowSize = 5;
sgbm.numberOfDisparities = 256;
sgbm.preFilterCap = 0;
sgbm.minDisparity = 0;
sgbm.uniquenessRatio = 1;
sgbm.speckleWindowSize = 150;
sgbm.speckleRange = 2;
sgbm.disp12MaxDiff = 10;
sgbm.fullDP = false;
sgbm.P1 = 1000;
sgbm.P2 = 2400;
cv::Mat disper;
sgbm(dst, dst2, disper);
normalize(disper, disparity, 0, 255, CV_MINMAX, CV_8U);*/
other_clock = clock() - other_clock;
time2 = ((float) (other_clock)) / CLOCKS_PER_SEC;
printf("time to compute : %f \n", time2);
//printf("compute done \n");
other_clock = clock();
/*png::image<png::rgb_pixel> segmentBoundaryImage;
makeSegmentBoundaryImage(dst, segmentImage, boundaryLabels, segmentBoundaryImage);
segmentBoundaryImage.write(ref + "__bound.png");*/
//disparityImage.write(ref + "__disparity.png");
other_clock = clock() - other_clock;
cv::imshow("vigne", disparity);
/*cv::Mat copy;
disparity.copyTo(copy);
STVFlow stvflow;
stvflow.compute(disparity,dst);*/
//copy.convertTo(copy, CV_8U,1/255.0,1/255.0);
/*cv::Mat dst455;
cv::threshold(disparity,dst455,50, 255, cv::THRESH_BINARY);*/
/*cv::imshow("bord",disparity);
cv::threshold(copy,copy,100, 255, cv::THRESH_BINARY);
cv::imshow("img test1",dst);*/
cv::waitKey(100);
cv::Mat img_disparity, img_rgb;
img_disparity = disparity;
img_rgb = left_image_rectified_crop;
img_disparity.convertTo(img_disparity, CV_8U, 1 / 255.0, 1 / 255.0); //A REMETRE EN SPS
//Create matrix that will contain 3D corrdinates of each pixel
cv::Mat recons3D(img_disparity.size(), CV_32FC3);
//Reproject image to 3D
std::cout << "Reprojecting image to 3D..." << std::endl;
cv::reprojectImageTo3D(img_disparity, recons3D, Q, false, CV_32F);
std::cout << "Creating Point Cloud..." << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_all_the_image_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
double px, py, pz;
uchar pr, pg, pb;
clock4 = clock();
for (int i = 0; i < img_rgb.rows; i++) {
uchar *rgb_ptr = img_rgb.ptr<uchar>(i);
uchar *disp_ptr = img_disparity.ptr<uchar>(i);
//double* recons_ptr = recons3D.ptr<double>(i);
for (int j = 0; j < img_rgb.cols; j++) {
//Get 3D coordinates
uchar d = disp_ptr[j];
if (d == 0) continue; //Discard bad pixels
double pw = -1.0 * static_cast<double>(d) * Q32 + Q33;
px = static_cast<double>(j) + Q03;
py = static_cast<double>(i) + Q13;
pz = Q23;
px = px / pw;
py = py / pw;
pz = pz / pw;
//Get RGB info
pb = rgb_ptr[3 * j];
pg = rgb_ptr[3 * j + 1];
pr = rgb_ptr[3 * j + 2];
//Insert info into point cloud structure
pcl::PointXYZRGB point;
point.x = px;
point.y = py;
point.z =-1* pz;
uint32_t rgb = (static_cast<uint32_t>(pr) << 16 |
static_cast<uint32_t>(pg) << 8 | static_cast<uint32_t>(pb));
point.rgb = *reinterpret_cast<float *>(&rgb);
//if(-1*pz < 90){
point_cloud_all_the_image_rgb->points.push_back(point);//}
pcl::PointXYZ pointx;
pointx.x = px;
pointx.y = py;
pointx.z = -1*pz;
if(-1*pz <90)
cloud->points.push_back(pointx);
}
}
point_cloud_all_the_image_rgb->width = (int) point_cloud_all_the_image_rgb->points.size();
point_cloud_all_the_image_rgb->height = 1;
cloud->width = (int) cloud->points.size();
cloud->height = 1;
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps a faire les pointcloud : %f \n", time4);
clock4 = clock();
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
//cloud = cloud_ptr;
std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ground(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgbseg(new pcl::PointCloud<pcl::PointXYZRGB>);
ExtractTheGround extractTheGround;
extractTheGround.compute(cloud, cloud_filtered, cloud_ground);
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps a sortir le sol : %f \n", time4);
clock4 = clock();
std::cout << "ytyt" << std::endl;
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clusters;
EuclidianClusters euclidianClusters;
euclidianClusters.compute(cloud_filtered, clusters);
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps a faire la seg euclid : %f \n", time4);
clock4 = clock();
/*pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgbseg_less(new pcl::PointCloud<pcl::PointXYZRGB>);
for(int i=0;i < cloud_filtered->points.size();i++){
for(int j=0;j < point_cloud_ptr->points.size();j++){
if(cloud_filtered->points[i].x == point_cloud_ptr->points[j].x && cloud_filtered->points[i].y == point_cloud_ptr->points[j].y &&
cloud_filtered->points[i].z == point_cloud_ptr->points[j].z){
cloud_rgbseg_less->points.push_back(point_cloud_ptr->points[j]);
break;
}
}
}
RGBSegmentation rgbseg;
rgbseg.compute(cloud_rgbseg_less, cloud_rgbseg);*/
//viewer.removeVisualizationCallable("jhjh");
viewer.removeVisualizationCallable("jhjha");
viewer.showCloud(point_cloud_all_the_image_rgb,"jhjha");
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps afficher image: %f \n", time4);
clock4 = clock();
//viewer.showCloud(cloud_rgbseg,"jhjh");
/*ClustersFilter clustersFilter;
clustersFilter.compute(clusters);*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cube(new pcl::PointCloud<pcl::PointXYZ>);
for(int i=0; i< clusters.size();i++) {
float minX, minY, minZ,maxZ,maxX,maxY,memY,memZ;
int nbrY,nbrZ;
bool dontgo = false;
bool zminbool = false;
for(int j=0; j < clusters.at(i)->points.size();j++) {
//pour chaque points du cluster en question, eh ouais !
float xpoint = clusters.at(i)->points[j].x;
float ypoint = clusters.at(i)->points[j].y;
float zpoint = clusters.at(i)->points[j].z;
if(j==0){
minX = xpoint; minY = ypoint; minZ = zpoint;
maxX = xpoint; maxY = ypoint; maxZ= zpoint;
memY = ypoint; memZ = zpoint;
}
if(memZ == zpoint ){
nbrZ++;
}else {
if (nbrZ >= 50) {
maxZ = zpoint;
if (!zminbool) {
zminbool = true;
minZ = zpoint;
}
}
nbrZ = 1;
}
if(memY != ypoint)
memY = ypoint;
if(memZ != zpoint) {
memZ = zpoint;
}
/*if(zpoint > maxZ)
maxZ = zpoint;*/
/*if(zpoint < minZ)
minZ = zpoint;*/
if(xpoint < minX)
minX= xpoint;
if(xpoint > maxX)
maxX = xpoint;
if( ypoint < minY)
minY = ypoint;
if( ypoint > maxY)
maxY = ypoint;
if(maxZ > (minZ +15))
break;
}
if (maxX - minX > 3 * (maxY - minY))
dontgo = true;
if(maxZ - minZ < 3)
dontgo = true;
if(dontgo)
continue;
float xdebut = minX, ydebut = minY, zdebut = minZ, xfin = maxX, yfin = maxY, zfin = maxZ;
//float xdebut = -10.20, ydebut = 0, zdebut = 12.20, xfin = -50.40, yfin = 15.24, zfin = -13.56;
if (ydebut > yfin) {
float tmp = ydebut;
ydebut = yfin;
yfin = tmp;
}
if (xdebut > xfin) {
float tmp = xdebut;
xdebut = xfin;
xfin = tmp;
}
if (zdebut > zfin) {
float tmp = zdebut;
zdebut = zfin;
zfin = tmp;
}
for (float i = xdebut; i <= xfin; i+=0.3) {
for (float j = ydebut; j <= yfin; j+=0.3) {
pcl::PointXYZ p;
pcl::PointXYZ p2;
p.x = i;
if (i == xdebut || (i >= xfin - 0.3 && i <= xfin + 0.3)) {
p.y = j;
}
else {
p.y = ydebut;
p2.y = yfin;
}
p.z = zfin;
p2.x = p.x;
p2.z = p.z;
cube->points.push_back(p);
cube->points.push_back(p2);
p.z = zdebut;
p2.z = zdebut;
cube->points.push_back(p);
cube->points.push_back(p2);
}
}
for(float j=ydebut; j <=yfin;j+=0.3){
for(float z =zdebut;z<=zfin;z+=0.3){
pcl::PointXYZ p;
pcl::PointXYZ p2;
p.x=xdebut;
p2.x=xfin;
p.y=j;
p.z=z;
p2.y=p.y;
p2.z=p.z;
if (j == ydebut || (j>= yfin - 0.3 && j <= yfin + 0.3)) {
cube->points.push_back(p);
cube->points.push_back(p2);
}
}
}
}
viewer.showCloud (cube,"oh");
/*pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_memory2 (new pcl::PointCloud<pcl::PointXYZRGB>);
int r2=15,b2=255,g3r=125;
for(int i = 0; i < clusters.size();i++){
uint32_t rgb = (static_cast<uint32_t>(r2) << 16 |
static_cast<uint32_t>(g3r) << 8 | static_cast<uint32_t>(b2));
for(int j=0;j< clusters.at(i)->points.size();j++){
clusters.at(i)->points[j].rgb = *reinterpret_cast<float*>(&rgb);
}
r2+=20;
b2+=100;
g3r+=5;
if(r2 >252)
r2=0;
if(g3r>252)
g3r=0;
if(b2>253)
b2=0;
*cloud_rgb_memory2 += *clusters.at(i);
}
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps a faire les clusters avant : %f \n", time4);
clock4 = clock();
std::cout<<"clusters size :"<<clusters.size() <<std::endl;
//viewer.showCloud (cloud_rgb_memory2,"eh eh eh");
clock4 = clock() - clock4;
time4 = ((float) (clock4)) / CLOCKS_PER_SEC;
printf("temps a faire les clusters aprés : %f \n", time4);
clock4 = clock();*/
}
//time
start = clock() - start;
time = ((float) ( start))/ CLOCKS_PER_SEC;
printf("time for all : %f and time without loss :%f \n",time,time - timtotal);
}
int main(int argc, char* argv[]) {
const int disp_size = 128;
sl::Camera zed;
sl::InitParameters initParameters;
initParameters.camera_resolution = sl::RESOLUTION_VGA;
sl::ERROR_CODE err = zed.open(initParameters);
if (err != sl::SUCCESS) {
std::cout << toString(err) << std::endl;
zed.close();
return 1;
}
const int width = static_cast<int>(zed.getResolution().width);
const int height = static_cast<int>(zed.getResolution().height);
sl::Mat d_zed_image_l(zed.getResolution(), sl::MAT_TYPE_8U_C1, sl::MEM_GPU);
sl::Mat d_zed_image_r(zed.getResolution(), sl::MAT_TYPE_8U_C1, sl::MEM_GPU);
const int input_depth = 8;
const int input_bytes = input_depth * width * height / 8;
const int output_depth = 8;
const int output_bytes = output_depth * width * height / 8;
sgm::StereoSGM sgm(width, height, disp_size, input_depth, output_depth, sgm::EXECUTE_INOUT_CUDA2CUDA);
cv::Mat disparity(height, width, CV_8U);
cv::Mat disparity_8u, disparity_color;
device_buffer d_image_l(input_bytes), d_image_r(input_bytes), d_disparity(output_bytes);
while (1) {
if (zed.grab() == sl::SUCCESS) {
zed.retrieveImage(d_zed_image_l, sl::VIEW_LEFT_GRAY, sl::MEM_GPU);
zed.retrieveImage(d_zed_image_r, sl::VIEW_RIGHT_GRAY, sl::MEM_GPU);
} else continue;
cudaMemcpy2D(d_image_l.data, width, d_zed_image_l.getPtr<uchar>(sl::MEM_GPU), d_zed_image_l.getStep(sl::MEM_GPU), width, height, cudaMemcpyDeviceToDevice);
cudaMemcpy2D(d_image_r.data, width, d_zed_image_r.getPtr<uchar>(sl::MEM_GPU), d_zed_image_r.getStep(sl::MEM_GPU), width, height, cudaMemcpyDeviceToDevice);
const auto t1 = std::chrono::system_clock::now();
sgm.execute(d_image_l.data, d_image_r.data, d_disparity.data);
cudaDeviceSynchronize();
const auto t2 = std::chrono::system_clock::now();
const auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
const double fps = 1e6 / duration;
cudaMemcpy(disparity.data, d_disparity.data, output_bytes, cudaMemcpyDeviceToHost);
disparity.convertTo(disparity_8u, CV_8U, 255. / disp_size);
cv::applyColorMap(disparity_8u, disparity_color, cv::COLORMAP_JET);
cv::putText(disparity_color, format_string("sgm execution time: %4.1f[msec] %4.1f[FPS]", 1e-3 * duration, fps),
cv::Point(50, 50), 2, 0.75, cv::Scalar(255, 255, 255));
cv::imshow("disparity", disparity_color);
const char c = cv::waitKey(1);
if (c == 27) // ESC
break;
}
zed.close();
return 0;
}
void callback(const stereo_msgs::DisparityImageConstPtr& disparityMsg)
{
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
{
ROS_ERROR("Input type must be disparity=32FC1");
return;
}
bool publish32f = pub32f_.getNumSubscribers();
bool publish16u = pub16u_.getNumSubscribers();
if(publish32f || publish16u)
{
// sensor_msgs::image_encodings::TYPE_32FC1
cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
cv::Mat depth32f;
cv::Mat depth16u;
if(publish32f)
{
depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
}
if(publish16u)
{
depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U);
}
for (int i = 0; i < disparity.rows; i++)
{
for (int j = 0; j < disparity.cols; j++)
{
float disparity_value = disparity.at<float>(i,j);
if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity)
{
// baseline * focal / disparity
float depth = disparityMsg->T * disparityMsg->f / disparity_value;
if(publish32f)
{
depth32f.at<float>(i,j) = depth;
}
if(publish16u)
{
depth16u.at<unsigned short>(i,j) = (unsigned short)(depth*1000.0f);
}
}
}
}
if(publish32f)
{
// convert to ROS sensor_msg::Image
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f);
sensor_msgs::Image depthMsg;
cvDepth.toImageMsg(depthMsg);
//publish the message
pub32f_.publish(depthMsg);
}
if(publish16u)
{
// convert to ROS sensor_msg::Image
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u);
sensor_msgs::Image depthMsg;
cvDepth.toImageMsg(depthMsg);
//publish the message
pub16u_.publish(depthMsg);
}
}
}
