bool DepthRegistrationCPU::init(const int deviceId)
{
createLookup();
proj(0, 0) = rotation.at<double>(0, 0);
proj(0, 1) = rotation.at<double>(0, 1);
proj(0, 2) = rotation.at<double>(0, 2);
proj(0, 3) = translation.at<double>(0, 0);
proj(1, 0) = rotation.at<double>(1, 0);
proj(1, 1) = rotation.at<double>(1, 1);
proj(1, 2) = rotation.at<double>(1, 2);
proj(1, 3) = translation.at<double>(1, 0);
proj(2, 0) = rotation.at<double>(2, 0);
proj(2, 1) = rotation.at<double>(2, 1);
proj(2, 2) = rotation.at<double>(2, 2);
proj(2, 3) = translation.at<double>(2, 0);
proj(3, 0) = 0;
proj(3, 1) = 0;
proj(3, 2) = 0;
proj(3, 3) = 1;
fx = cameraMatrixRegistered.at<double>(0, 0);
fy = cameraMatrixRegistered.at<double>(1, 1);
cx = cameraMatrixRegistered.at<double>(0, 2) + 0.5;
cy = cameraMatrixRegistered.at<double>(1, 2) + 0.5;
return true;
}
bool DepthRegistrationCPU::init()
{
createLookup();
cv::initUndistortRectifyMap(cameraMatrixDepth, cv::Mat(), cv::Mat(), cameraMatrixColor, sizeColor, CV_16SC2, map1, map2);
return true;
}
libmaus2::util::LookupIntervalTree::LookupIntervalTree(
::libmaus2::autoarray::AutoArray < std::pair<uint64_t,uint64_t> > const & rH,
unsigned int const rrangebits,
unsigned int const rsublookupbits
) : H(rH.clone()), I(H,0,H.size()), rangebits(rrangebits), sublookupbits(rsublookupbits),
L(createLookup()), lookupshift ( rangebits - sublookupbits )
{
}
void start(const Mode mode)
{
this->mode = mode;
running = true;
std::string topicCameraInfoColor = topicColor.substr(0, topicColor.rfind('/')) + "/camera_info";
std::string topicCameraInfoDepth = topicDepth.substr(0, topicDepth.rfind('/')) + "/camera_info";
// std::cout << "test1..." << std::endl;
image_transport::TransportHints hints("raw");
subImageColor = new image_transport::SubscriberFilter(it, topicColor, queueSize, hints);
subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, queueSize, hints);
subCameraInfoColor = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoColor, queueSize);
subCameraInfoDepth = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoDepth, queueSize);
// std::cout << "test2..." << std::endl;
syncExact = new message_filters::Synchronizer<ExactSyncPolicy>(ExactSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
syncExact->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
spinner.start();
std::cout << "waiting for kinect2_bridge..." << std::endl;
std::chrono::milliseconds duration(1);
while(!updateImage || !updateCloud)
{
if(!ros::ok())
{
return;
}
std::this_thread::sleep_for(duration);
}
std::cout << "kinect2_bridge is running.." << std::endl;
cloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>());
cloud->height = color.rows;
cloud->width = color.cols;
cloud->is_dense = false;
cloud->points.resize(cloud->height * cloud->width);
createLookup(this->color.cols, this->color.rows);
//
// // Create the filtering object
// // pcl::PassThrough<pcl::PointXYZRGBA> pass;
// // pass.setInputCloud (cloud);
// // pass.setFilterFieldName ("z");
// // pass.setFilterLimits (0.0, 1.0);
// // //pass.setFilterLimitsNegative (true);
// // pass.filter (*cloud);
cloudViewer();
}
// Create the lookup file
void converter::createLookup()
{
const char *fnameIn = getenv("SLOT_DETAILS_FILE");
if ( fnameIn==NULL ) {
errlogPrintf("Environment variable SLOT_DETAILS_FILE not set\n");
return;
}
loadSlotDetails(fnameIn);
const char *fnameOut = getenv("SAMPLE_LKUP_FILE");
if ( fnameOut==NULL ) {
errlogPrintf("Environment variable SAMPLE_LKUP_FILE not set\n");
return;
}
FILE *fpOut = fopen(fnameOut, "w");
if ( fpOut==NULL ) {
errlogPrintf("Unable to open %s\n", fnameOut);
return;
}
createLookup(fpOut);
fclose(fpOut);
}
