@@ -113,40 +113,26 @@ FrameworkReturnCode SolAR3DPointsViewerOpengl::display (const std::vector<SRef<C
113113 if (m_firstDisplay)
114114 {
115115 // Compute the center point of the point cloud
116- Point3Df minPoint, maxPoint;
117- maxPoint (0 )=(std::numeric_limits<size_t >::lowest)();
118- maxPoint (1 )=(std::numeric_limits<size_t >::lowest)();
119- maxPoint (2 )=(std::numeric_limits<size_t >::lowest)();
120- minPoint (0 )=(std::numeric_limits<size_t >::max)();
121- minPoint (1 )=(std::numeric_limits<size_t >::max)();
122- minPoint (2 )=(std::numeric_limits<size_t >::max)();
123-
124- for (int i = 0 ; i < m_points.size (); i++)
116+ std::vector<float > xValues, yValues, zValues;
117+ int nbPoints = m_points.size ();
118+ for (int i = 0 ; i < nbPoints; i++)
125119 {
126- if (points[i]->getX () > maxPoint (0 )) maxPoint (0 )=points[i]->getX ();
127- if (points[i]->getY () > maxPoint (1 )) maxPoint (1 )=points[i]->getY ();
128- if (points[i]->getZ () > maxPoint (2 )) maxPoint (2 )=points[i]->getZ ();
129- if (points[i]->getX () < minPoint (0 )) minPoint (0 )=points[i]->getX ();
130- if (points[i]->getY () < minPoint (1 )) minPoint (1 )=points[i]->getY ();
131- if (points[i]->getZ () < minPoint (2 )) minPoint (2 )=points[i]->getZ ();
120+ xValues.push_back (m_points[i]->getX ());
121+ yValues.push_back (m_points[i]->getY ());
122+ zValues.push_back (m_points[i]->getZ ());
132123 }
133- Vector3f sceneDiagonal;
124+ std::sort (xValues.begin (), xValues.end ());
125+ std::sort (yValues.begin (), yValues.end ());
126+ std::sort (zValues.begin (), zValues.end ());
134127
135128 // Center the scene on the center of the point cloud
136- m_sceneCenter = Point3Df ((minPoint (0 )+maxPoint (0 ))/2 .0f , -(minPoint (1 )+maxPoint (1 ))/2 .0f , -(minPoint (2 )+maxPoint (2 ))/2 .0f );
137-
138- // Add the camera to the box of the scene
139- if (m_cameraPose (0 ,3 ) > maxPoint (0 )) maxPoint (0 )=m_cameraPose (0 ,3 );
140- if (m_cameraPose (1 ,3 ) > maxPoint (1 )) maxPoint (1 )=m_cameraPose (1 ,3 );
141- if (m_cameraPose (2 ,3 ) > maxPoint (2 )) maxPoint (2 )=m_cameraPose (2 ,3 );
142- if (m_cameraPose (0 ,3 ) < minPoint (0 )) minPoint (0 )=m_cameraPose (0 ,3 );
143- if (m_cameraPose (1 ,3 ) < minPoint (1 )) minPoint (1 )=m_cameraPose (1 ,3 );
144- if (m_cameraPose (2 ,3 ) < minPoint (2 )) minPoint (2 )=m_cameraPose (2 ,3 );
129+ m_sceneCenter = Point3Df (xValues[nbPoints / 2 ], yValues[nbPoints / 2 ], zValues[nbPoints / 2 ]);
145130
146131 // Copmute the diagonal of the box to define the scene Size
147- sceneDiagonal (0 ) = maxPoint (0 ) - minPoint (0 );
148- sceneDiagonal (1 ) = maxPoint (1 ) - minPoint (1 );
149- sceneDiagonal (2 ) = maxPoint (2 ) - minPoint (2 );
132+ Vector3f sceneDiagonal;
133+ sceneDiagonal (0 ) = std::abs (xValues[nbPoints * 0.99 ] - xValues[nbPoints * 0.01 ]);
134+ sceneDiagonal (1 ) = std::abs (yValues[nbPoints * 0.99 ] - yValues[nbPoints * 0.01 ]);
135+ sceneDiagonal (2 ) = std::abs (zValues[nbPoints * 0.99 ] - zValues[nbPoints * 0.01 ]);
150136 m_sceneSize = sceneDiagonal.norm ();
151137
152138 // Set the camera according to the center and the size of the scene.
@@ -173,19 +159,11 @@ FrameworkReturnCode SolAR3DPointsViewerOpengl::display( const SRef<PointCloud> p
173159 const std::vector<Transform3Df> & keyframePoses2)
174160{
175161 std::vector<SRef<CloudPoint>> points_3Df;
176- const std::vector<CloudPoint> points = pointCloud->getConstPointCloud ();
177- for (auto & point : points)
178- {
179- points_3Df.push_back (xpcf::utils::make_shared<CloudPoint>(point));
180- }
162+ pointCloud->getAllPoints (points_3Df);
181163 std::vector<SRef<CloudPoint>> points2_3Df;
182164 if (pointCloud2 != nullptr )
183165 {
184- const std::vector<CloudPoint> points2 = pointCloud2->getConstPointCloud ();
185- for (auto & point2 : points2)
186- {
187- points2_3Df.push_back (xpcf::utils::make_shared<CloudPoint>(point2));
188- }
166+ pointCloud2->getAllPoints (points2_3Df);
189167 }
190168 return display (points_3Df, pose, keyframePoses, framePoses, points2_3Df, keyframePoses2);
191169}
0 commit comments