int main(void)
{
// Engine initialization
TinyRender::EngineManager engine;
engine.set_viewport(0, 0, width, height);
engine.set_depth(width);
// TODO: (alxe) simplify api!!
TinyRender::Vec3f light_vec;
light_vec[0] = 0.0; light_vec[1] = 0.0; light_vec[2] = -1.0;
engine.set_light_direction(light_vec);
// Shader creation
std::unique_ptr<TinyRender::Shader> shader = std::make_unique<TinyRender::DummyShader>(engine);
std::vector<std::vector<int>> z_buffer(height, std::vector<int>(width, std::numeric_limits<int>::min()));
TinyRender::TGAImage tga_image(width, height, TinyRender::TGAImage::ImageFormat_RGB);
TinyRender::Model model("../../Models/AfricanHead.obj");
TinyRender::TGAImage text_map("../../Models/AfricanHeadDiffuse.tga");
std::ofstream out_tga_file("../../Temp/TGATEST.tga", std::ios::binary);
// Buffer of screen points
std::array<TinyRender::Vec3i, 3> screen_points;
for (size_t tr_id = 0; tr_id < model.triangles().size(); ++tr_id)
{
const TinyRender::Vec3f& p1 = model.vertices()[model.triangles()[tr_id].vertices[0]];
const TinyRender::Vec3f& uv1 = model.uv_vertices()[model.triangles()[tr_id].uv_vertices[0]];
const TinyRender::Vec3f& p2 = model.vertices()[model.triangles()[tr_id].vertices[1]];
const TinyRender::Vec3f& uv2 = model.uv_vertices()[model.triangles()[tr_id].uv_vertices[1]];
const TinyRender::Vec3f& p3 = model.vertices()[model.triangles()[tr_id].vertices[2]];
const TinyRender::Vec3f& uv3 = model.uv_vertices()[model.triangles()[tr_id].uv_vertices[2]];
const TinyRender::Vec3i& p1i = engine.transform(p1);
const TinyRender::Vec3i& p2i = engine.transform(p2);
const TinyRender::Vec3i& p3i = engine.transform(p3);
const float u1x = uv1[0] * text_map.width();
const float u1y = uv1[1] * text_map.height();
const float u2x = uv2[0] * text_map.width();
const float u2y = uv2[1] * text_map.height();
const float u3x = uv3[0] * text_map.width();
const float u3y = uv3[1] * text_map.height();
TinyRender::Vec3f norm_vec = vector_product((p3 - p1), (p2 - p1));
norm_vec.normalize();
const float intense = light_vec * norm_vec;
if (intense > 0)
{
TinyRender::render_triangle(p1i[0], p1i[1], p1i[2], u1x, u1y,
p2i[0], p2i[1], p2i[2], u2x, u2y,
p3i[0], p3i[1], p3i[2], u3x, u3y,
tga_image, text_map, z_buffer, intense);
}
}
tga_image << out_tga_file;
out_tga_file.close();
return 0;
}
void GUIPlugin::publishMapImage()
{
// clear image
map_image_ = cv::Mat(map_image_.rows, map_image_.cols, CV_8UC3, cv::Scalar(10, 10, 10));
// Draw world model shapes
cv::Mat z_buffer(map_image_.rows, map_image_.cols, CV_32FC1, 0.0);
for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity)
{
const ed::EntityConstPtr& e = *it_entity;
if (e->shape() && e->id() != "floor")
{
cv::Scalar color = idToColor(e->id());
cv::Vec3b color_vec(0.5 * color[0], 0.5 * color[1], 0.5 * color[2]);
geo::Pose3D pose = projector_pose_.inverse() * e->pose();
geo::RenderOptions opt;
opt.setMesh(e->shape()->getMesh(), pose);
ColorRenderResult res(map_image_, z_buffer, color_vec, projector_pose_.t.z - 3, projector_pose_.t.z + 1);
// Render
projector_.render(opt, res);
}
}
for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity)
{
const ed::EntityConstPtr& e = *it_entity;
const pcl::PointCloud<pcl::PointXYZ>& chull_points = e->convexHull().chull;
cv::Scalar color = idToColor(e->id());
int thickness = 1;
if (selected_id_ == e->id())
thickness = 3;
// draw the polygon
if (!chull_points.empty())
{
// Lower polygon
for(unsigned int i = 0; i < chull_points.size(); ++i)
{
int j = (i + 1) % chull_points.size();
const pcl::PointXYZ& p1 = chull_points[i];
const pcl::PointXYZ& p2 = chull_points[j];
cv::line(map_image_,
coordinateToPixel(p1.x, p1.y, e->convexHull().min_z),
coordinateToPixel(p2.x, p2.y, e->convexHull().min_z),
0.3 * color, thickness);
}
// Edges in between
for(unsigned int i = 0; i < chull_points.size(); ++i)
{
const pcl::PointXYZ p = chull_points[i];
cv::line(map_image_,
coordinateToPixel(p.x, p.y, e->convexHull().min_z),
coordinateToPixel(p.x, p.y, e->convexHull().max_z),
0.5 * color, thickness);
}
// Upper polygon
for(unsigned int i = 0; i < chull_points.size(); ++i)
{
int j = (i + 1) % chull_points.size();
const pcl::PointXYZ& p1 = chull_points[i];
const pcl::PointXYZ& p2 = chull_points[j];
cv::line(map_image_,
coordinateToPixel(p1.x, p1.y, e->convexHull().max_z),
coordinateToPixel(p2.x, p2.y, e->convexHull().max_z),
color, thickness);
}
if (e->type() == "person")
{
cv::circle(map_image_, coordinateToPixel(e->convexHull().center_point), 15, cv::Scalar(255, 0, 0), 10);
}
}
}
// Find the global most recent measurement
ed::MeasurementConstPtr last_measurement;
for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity)
{
const ed::EntityConstPtr& e = *it_entity;
if (e->lastMeasurement() && (!last_measurement || e->lastMeasurement()->timestamp() > e->lastMeasurement()->timestamp()))
last_measurement = e->lastMeasurement();
}
if (last_measurement)
{
const geo::Pose3D& sensor_pose = last_measurement->sensorPose();
// Draw sensor origin
const geo::Vector3& p = sensor_pose.getOrigin();
cv::Point2i p_2d = coordinateToPixel(p);
cv::circle(map_image_, p_2d, 10, cv::Scalar(255, 255, 255));
rgbd::View view(*last_measurement->image(), 100); // width doesnt matter; we'll go back to world coordinates anyway
geo::Vector3 p1 = view.getRasterizer().project2Dto3D(0, 0) * 3;
geo::Vector3 p2 = view.getRasterizer().project2Dto3D(view.getWidth() - 1, 0) * 3;
geo::Vector3 p3 = view.getRasterizer().project2Dto3D(0, view.getHeight() - 1) * 3;
geo::Vector3 p4 = view.getRasterizer().project2Dto3D(view.getWidth() - 1, view.getHeight() - 1) * 3;
cv::Point2i p1_2d = coordinateToPixel(sensor_pose * p1);
cv::Point2i p2_2d = coordinateToPixel(sensor_pose * p2);
cv::Point2i p3_2d = coordinateToPixel(sensor_pose * p3);
cv::Point2i p4_2d = coordinateToPixel(sensor_pose * p4);
cv::Scalar fustrum_color(100, 100, 100);
cv::line(map_image_, p_2d, p1_2d, fustrum_color);
cv::line(map_image_, p_2d, p2_2d, fustrum_color);
cv::line(map_image_, p_2d, p3_2d, fustrum_color);
cv::line(map_image_, p_2d, p4_2d, fustrum_color);
cv::line(map_image_, p1_2d, p2_2d, fustrum_color);
cv::line(map_image_, p1_2d, p3_2d, fustrum_color);
cv::line(map_image_, p2_2d, p4_2d, fustrum_color);
cv::line(map_image_, p3_2d, p4_2d, fustrum_color);
}
if (t_last_click_ > ros::Time(0) && t_last_click_ > ros::Time::now() - ros::Duration(1))
{
cv::circle(map_image_, p_click_, 20, cv::Scalar(0, 0, 255), 5);
}
// Convert to binary
tue_serialization::Binary msg;
if (imageToBinary(map_image_, msg.data, IMAGE_COMPRESSION_PNG))
{
pub_image_map_.publish(msg);
}
// cv::imshow("gui", map_image_);
// cv::waitKey(3);
}
