diff --git a/core/src/manual_pose_graph_loop_closure.cpp b/core/src/manual_pose_graph_loop_closure.cpp index 9c8555ef..84585047 100644 --- a/core/src/manual_pose_graph_loop_closure.cpp +++ b/core/src/manual_pose_graph_loop_closure.cpp @@ -424,8 +424,22 @@ void ManualPoseGraphLoopClosure::Render( { if (i >= 0 && i < point_clouds_container.point_clouds.size() && point_clouds_container.point_clouds.size() > 0) { - ObservationPicking observation_picking; - point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, false); + // ObservationPicking observation_picking; + // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, false); + Eigen::Affine3d m_src = point_clouds_container.point_clouds.at(i).m_pose; + + if (render_source_as_red_target_as_blue) + { + float color[3]; + color[0] = 1.0; + color[1] = 0.0; + color[2] = 0.0; + point_clouds_container.point_clouds.at(i).render(m_src, 1, 1, color); + } + else + { + point_clouds_container.point_clouds.at(i).render(m_src, 1, 1, point_clouds_container.point_clouds.at(i).render_color); + } } } @@ -433,8 +447,22 @@ void ManualPoseGraphLoopClosure::Render( { if (i >= 0 && i < point_clouds_container.point_clouds.size() && point_clouds_container.point_clouds.size() > 0) { - ObservationPicking observation_picking; - point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, false); + // ObservationPicking observation_picking; + // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, false); + Eigen::Affine3d m_src = point_clouds_container.point_clouds.at(i).m_pose; + + if (render_source_as_red_target_as_blue) + { + float color[3]; + color[0] = 0.0; + color[1] = 0.0; + color[2] = 1.0; + point_clouds_container.point_clouds.at(i).render(m_src, 1, 1, color); + } + else + { + point_clouds_container.point_clouds.at(i).render(m_src, 1, 1, point_clouds_container.point_clouds.at(i).render_color); + } } } }