Merge pull request #3137 from savuor:backport_levmarqfromscratch

savuor · web-flow · commit 49e8f123ca08 · 2021-12-24T23:01:26.000Z
HashTSDF fixes backported

* HashTSDF fixes backported

* typo fixed
diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp
@@ -796,11 +796,11 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor
                                 if (voxel.tsdf != -128 && voxel.weight != 0)
                                 {
                                     Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx);
-                                    localPoints.push_back(toPtype(point));
+                                    localPoints.push_back(toPtype(this->pose * point));
                                     if (needNormals)
                                     {
                                         Point3f normal = volume.getNormalVoxel(point);
-                                        localNormals.push_back(toPtype(normal));
+                                        localNormals.push_back(toPtype(this->pose.rotation() * normal));
                                     }
                                 }
                             }
@@ -1685,11 +1685,11 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor
                             {
                                 Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx);
 
-                                localPoints.push_back(toPtype(point));
+                                localPoints.push_back(toPtype(this->pose * point));
                                 if (needNormals)
                                 {
                                     Point3f normal = volume.getNormalVoxel(point);
-                                    localNormals.push_back(toPtype(normal));
+                                    localNormals.push_back(toPtype(this->pose.rotation() * normal));
                                 }
                             }
                         }

Original file line number	Diff line number	Diff line change
`@@ -796,11 +796,11 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor`
`796`	`796`	`if (voxel.tsdf != -128 && voxel.weight != 0)`
`797`	`797`	`{`
`798`	`798`	`Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx);`
`799`		`- localPoints.push_back(toPtype(point));`
	`799`	`+ localPoints.push_back(toPtype(this->pose * point));`
`800`	`800`	`if (needNormals)`
`801`	`801`	`{`
`802`	`802`	`Point3f normal = volume.getNormalVoxel(point);`
`803`		`- localNormals.push_back(toPtype(normal));`
	`803`	`+ localNormals.push_back(toPtype(this->pose.rotation() * normal));`
`804`	`804`	`}`
`805`	`805`	`}`
`806`	`806`	`}`
`@@ -1685,11 +1685,11 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor`
`1685`	`1685`	`{`
`1686`	`1686`	`Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx);`
`1687`	`1687`
`1688`		`- localPoints.push_back(toPtype(point));`
	`1688`	`+ localPoints.push_back(toPtype(this->pose * point));`
`1689`	`1689`	`if (needNormals)`
`1690`	`1690`	`{`
`1691`	`1691`	`Point3f normal = volume.getNormalVoxel(point);`
`1692`		`- localNormals.push_back(toPtype(normal));`
	`1692`	`+ localNormals.push_back(toPtype(this->pose.rotation() * normal));`
`1693`	`1693`	`}`
`1694`	`1694`	`}`
`1695`	`1695`	`}`