void DiffuseGain::applyPtmRGB(const PyramidCoeff& redCoeff, const PyramidCoeff& greenCoeff, const PyramidCoeff& blueCoeff, const QSize* mipMapSize, const PyramidNormals& normals, const RenderingInfo& info, unsigned char* buffer)
{
int offsetBuf = 0;
const PTMCoefficient* redPtr = redCoeff.getLevel(info.level);
const PTMCoefficient* greenPtr = greenCoeff.getLevel(info.level);
const PTMCoefficient* bluePtr = blueCoeff.getLevel(info.level);
const vcg::Point3f* normalsPtr = normals.getLevel(info.level);
// Creates the output texture.
#pragma omp parallel for schedule(static,CHUNK)
for (int y = info.offy; y < info.offy + info.height; y++)
{
int offsetBuf = ((y-info.offy)*info.width)<<2;
int offset = y * mipMapSize[info.level].width() + info.offx;
for (int x = info.offx; x < info.offx + info.width; x++)
{
buffer[offsetBuf + 0] = tobyte(applyModel(&(redPtr[offset][0]), normalsPtr[offset].X(), normalsPtr[offset].Y(), info.light.X(), info.light.Y()));
buffer[offsetBuf + 1] = tobyte(applyModel(&(greenPtr[offset][0]), normalsPtr[offset].X(), normalsPtr[offset].Y(), info.light.X(), info.light.Y()));
buffer[offsetBuf + 2] = tobyte(applyModel(&(bluePtr[offset][0]), normalsPtr[offset].X(), normalsPtr[offset].Y(), info.light.X(), info.light.Y()));
buffer[offsetBuf + 3] = 255;
offset++;
offsetBuf += 4;
}
}
//qDebug() << "gain = " << DiffuseGain::gain;
}
void DiffuseGain::applyPtmLRGB(const PyramidCoeff& coeff, const PyramidRGB& rgb, const QSize* mipMapSize, const PyramidNormals& normals, const RenderingInfo& info, unsigned char* buffer)
{
int offsetBuf = 0;
const unsigned char* rgbPtr = rgb.getLevel(info.level);
const PTMCoefficient* coeffPtr = coeff.getLevel(info.level);
const vcg::Point3f* normalsPtr = normals.getLevel(info.level);
// Creates the output texture.
#pragma omp parallel for schedule(static,CHUNK)
for (int y = info.offy; y < info.offy + info.height; y++)
{
int offsetBuf = ((y-info.offy)*info.width)<<2;
int offset = y * mipMapSize[info.level].width() + info.offx;
for (int x = info.offx; x < info.offx + info.width; x++)
{
//int offset = y * mipMapSize[info.level].width() + x;
float lum = applyModel(&(coeffPtr[offset][0]), normalsPtr[offset].X(), normalsPtr[offset].Y(), info.light.X(), info.light.Y()) / 256.0f;
int offset3 = offset*3;
for (int i = 0; i < 3; i++)
buffer[offsetBuf + i] = tobyte(rgbPtr[offset3 + i] * lum);
buffer[offsetBuf + 3] = 255;
offsetBuf += 4;
offset++;
}
}
//qDebug() << "gain = " << DiffuseGain::gain;
}
void DepthCalibration::calibrate(
const sensor_msgs::Image::ConstPtr& msg,
const sensor_msgs::CameraInfo::ConstPtr& camera_info)
{
boost::mutex::scoped_lock lock(mutex_);
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
}
catch (cv_bridge::Exception& e)
{
JSK_NODELET_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat image = cv_ptr->image;
cv::Mat output_image = image.clone();
double cu = camera_info->P[2];
double cv = camera_info->P[6];
for(int v = 0; v < image.rows; v++) {
for(int u = 0; u < image.cols; u++) {
float z = image.at<float>(v, u);
if (isnan(z)) {
output_image.at<float>(v, u) = z;
}
else {
output_image.at<float>(v, u) = applyModel(z, u, v, cu, cv);
}
//JSK_NODELET_INFO("z: %f", z);
}
}
sensor_msgs::Image::Ptr ros_image = cv_bridge::CvImage(msg->header, "32FC1", output_image).toImageMsg();
pub_.publish(ros_image);
}
